Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1646 → Rev 1647

/beta/Code Redesign killagreg/fc.c
241,35 → 241,34
AdBiasAccRoll = 0;
AdBiasAccTop = 0;
 
BiasHiResGyroNick = 0;
BiasHiResGyroNick = 0;
BiasHiResGyroRoll = 0;
AdBiasGyroYaw = 0;
 
FCParam.AxisCoupling1 = 0;
FCParam.AxisCoupling2 = 0;
FCParam.AxisCoupling1 = 0;
FCParam.AxisCoupling2 = 0;
 
ExpandBaro = 0;
ExpandBaro = 0;
 
TransmitBlConfig = 1;
motor_read = 0;
I2C_SendBLConfig();
 
// sample values with bias set to zero
Delay_ms_Mess(100);
Delay_ms_Mess(100);
 
if(BoardRelease == 13) SearchDacGyroOffset();
if(BoardRelease == 13) SearchDacGyroOffset();
 
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR)) // air pressure sensor installed?
{
if((ParamSet.Config0 & CFG0_AIRPRESS_SENSOR)) // air pressure sensor installed?
{
if((AdAirPressure > AIR_PRESSURE_SEARCH_MAX) || (AdAirPressure < AIR_PRESSURE_SEARCH_MIN)) SearchAirPressureOffset();
}
}
 
// determine gyro bias by averaging (require no rotation movement)
#define GYRO_BIAS_AVERAGE 32
Sum_1 = 0;
// determine gyro bias by averaging (require no rotation movement)
#define GYRO_BIAS_AVERAGE 32
Sum_1 = 0;
Sum_2 = 0;
Sum_3 = 0;
for(i=0; i < GYRO_BIAS_AVERAGE; i++)
{
for(i=0; i < GYRO_BIAS_AVERAGE; i++)
{
Delay_ms_Mess(10);
Sum_1 += AdValueGyroNick * HIRES_GYRO_AMPLIFY;
Sum_2 += AdValueGyroRoll * HIRES_GYRO_AMPLIFY;
279,8 → 278,8
BiasHiResGyroRoll = (int16_t)((Sum_2 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
AdBiasGyroYaw = (int16_t)((Sum_3 + GYRO_BIAS_AVERAGE / 2) / GYRO_BIAS_AVERAGE);
 
if(AccAdjustment != NO_ACC_CALIB)
{
if(AccAdjustment != NO_ACC_CALIB)
{
// determine acc bias by averaging (require horizontal adjustment in nick and roll attitude)
#define ACC_BIAS_AVERAGE 10
Sum_1 = 0;
302,14 → 301,14
SetParamWord(PID_ACC_NICK, (uint16_t)AdBiasAccNick);
SetParamWord(PID_ACC_ROLL, (uint16_t)AdBiasAccRoll);
SetParamWord(PID_ACC_TOP, (uint16_t)AdBiasAccTop);
}
else // restore from eeprom
{
}
else // restore from eeprom
{
AdBiasAccNick = (int16_t)GetParamWord(PID_ACC_NICK);
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP);
AdBiasAccRoll = (int16_t)GetParamWord(PID_ACC_ROLL);
AdBiasAccTop = (int16_t)GetParamWord(PID_ACC_TOP);
 
if(((uint16_t)AdBiasAccNick > 2048) || ((uint16_t)AdBiasAccRoll > 2048) || ((uint16_t)AdBiasAccTop > 1024))
if(((uint16_t)AdBiasAccNick > 2048) || ((uint16_t)AdBiasAccRoll > 2048) || ((uint16_t)AdBiasAccTop > 1024))
{
printf("\n\rACC not calibrated!\r\n");
AdBiasAccNick = 1024;
316,55 → 315,55
AdBiasAccRoll = 1024;
AdBiasAccTop = 725;
}
}
}
// offset for height reading
StartAirPressure = AirPressure;
StartAirPressure = AirPressure;
 
// setting acc bias values has an influence in the analog.c ISR
// therefore run measurement for 100ms to achive stable readings
// setting acc bias values has an influence in the analog.c ISR
// therefore run measurement for 100ms to achive stable readings
Delay_ms_Mess(100);
 
ReadingVario = 0;
 
// reset acc averaging and integrals
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
AccTop = AdValueAccTop;
ReadingIntegralTop = AdValueAccTop * 1024;
// reset acc averaging and integrals
AccNick = ACC_AMPLIFY * (int32_t)AdValueAccNick;
AccRoll = ACC_AMPLIFY * (int32_t)AdValueAccRoll;
AccTop = AdValueAccTop;
ReadingIntegralTop = AdValueAccTop * 1024;
 
// and gyro readings
GyroNick = 0;
GyroRoll = 0;
GyroYaw = 0;
GyroYaw = 0;
 
// reset gyro integrals to acc guessing
IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
// reset gyro integrals to acc guessing
IntegralGyroNick = ParamSet.GyroAccFactor * (int32_t)AccNick;
IntegralGyroRoll = ParamSet.GyroAccFactor * (int32_t)AccRoll;
//ReadingIntegralGyroNick = IntegralGyroNick;
//ReadingIntegralGyroRoll = IntegralGyroRoll;
ReadingIntegralGyroNick2 = IntegralGyroNick;
ReadingIntegralGyroRoll2 = IntegralGyroRoll;
ReadingIntegralGyroYaw = 0;
ReadingIntegralGyroNick2 = IntegralGyroNick;
ReadingIntegralGyroRoll2 = IntegralGyroRoll;
ReadingIntegralGyroYaw = 0;
 
// update compass course to current heading
CompassCourse = CompassHeading;
// Inititialize YawGyroIntegral value with current compass heading
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
YawGyroDrift = 0;
CompassCourse = CompassHeading;
// Inititialize YawGyroIntegral value with current compass heading
YawGyroHeading = (int32_t)CompassHeading * GYRO_DEG_FACTOR;
YawGyroDrift = 0;
 
BeepTime = 50;
BeepTime = 50;
 
TurnOver180Nick = ((int32_t)ParamSet.AngleTurnOverNick * 2500L) + 15000L;
TurnOver180Roll = ((int32_t)ParamSet.AngleTurnOverRoll * 2500L) + 15000L;
 
ExternHeightValue = 0;
ExternHeightValue = 0;
 
GPSStickNick = 0;
GPSStickRoll = 0;
GPSStickNick = 0;
GPSStickRoll = 0;
 
FCFlags |= FCFLAG_CALIBRATE;
FCFlags |= FCFLAG_CALIBRATE;
 
FCParam.KalmanK = -1;
FCParam.KalmanK = -1;
FCParam.KalmanMaxDrift = 0;
FCParam.KalmanMaxFusion = 32;
 
382,16 → 381,16
/************************************************************************/
void Mean(void)
{
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
int16_t FilterGyroNick, FilterGyroRoll;
int32_t tmpl = 0, tmpl2 = 0, tmp13 = 0, tmp14 = 0;
int16_t FilterGyroNick, FilterGyroRoll;
static int16_t Last_GyroRoll = 0, Last_GyroNick = 0;
int16_t d2Nick, d2Roll;
int32_t AngleNick, AngleRoll;
int16_t CouplingNickRoll = 0, CouplingRollNick = 0;
 
// Get bias free gyro readings
GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
// Get bias free gyro readings
GyroNick = HiResGyroNick / HIRES_GYRO_AMPLIFY; // unfiltered gyro rate
FilterGyroNick = FilterHiResGyroNick / HIRES_GYRO_AMPLIFY; // use filtered gyro rate
 
// handle rotation rates that violate adc ranges
if(AdValueGyroNick < 15) GyroNick = -1000;
425,7 → 424,7
 
GyroYaw = AdBiasGyroYaw - AdValueGyroYaw;
 
// Acceleration Sensor
// Acceleration Sensor
// lowpass acc measurement and scale AccNick/AccRoll by a factor of ACC_AMPLIFY to have a better resolution
AccNick = ((int32_t)AccNick * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccNick))) / 4L;
AccRoll = ((int32_t)AccRoll * 3L + ((ACC_AMPLIFY * (int32_t)AdValueAccRoll))) / 4L;
432,12 → 431,12
AccTop = ((int32_t)AccTop * 3L + ((int32_t)AdValueAccTop)) / 4L;
 
// sum acc sensor readings for later averaging
MeanAccNick += ACC_AMPLIFY * AdValueAccNick;
MeanAccRoll += ACC_AMPLIFY * AdValueAccRoll;
MeanAccNick += ACC_AMPLIFY * AdValueAccNick;
MeanAccRoll += ACC_AMPLIFY * AdValueAccRoll;
 
NaviAccNick += AdValueAccNick;
NaviAccRoll += AdValueAccRoll;
NaviCntAcc++;
NaviAccNick += AdValueAccNick;
NaviAccRoll += AdValueAccRoll;
NaviCntAcc++;
 
 
// enable ADC to meassure next readings, before that point all variables should be read that are written by the ADC ISR
503,8 → 502,8
 
// Yaw
 
// limit YawGyroHeading proportional to 0° to 360°
if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360° Wrap
// limit YawGyroHeading proportional to 0° to 360°
if(YawGyroHeading >= (360L * GYRO_DEG_FACTOR)) YawGyroHeading -= 360L * GYRO_DEG_FACTOR; // 360° Wrap
if(YawGyroHeading < 0) YawGyroHeading += 360L * GYRO_DEG_FACTOR;
 
// Roll
535,11 → 534,11
ReadingIntegralGyroNick2 = ReadingIntegralGyroNick;
}
 
IntegralGyroYaw = ReadingIntegralGyroYaw;
IntegralGyroNick = ReadingIntegralGyroNick;
IntegralGyroRoll = ReadingIntegralGyroRoll;
IntegralGyroNick2 = ReadingIntegralGyroNick2;
IntegralGyroRoll2 = ReadingIntegralGyroRoll2;
IntegralGyroYaw = ReadingIntegralGyroYaw;
IntegralGyroNick = ReadingIntegralGyroNick;
IntegralGyroRoll = ReadingIntegralGyroRoll;
IntegralGyroNick2 = ReadingIntegralGyroNick2;
IntegralGyroRoll2 = ReadingIntegralGyroRoll2;
 
 
#define D_LIMIT 128
1602,7 → 1601,7
}
}
else // no switchable height control
{ // the height control is always active and the set point is defined by the parameter
{ // the height control is always active and the set point is defined by the parameter
if( !(BaroFlags & (BARO_LIMIT_MIN|BARO_LIMIT_MAX)) )
{
SetPointHeight = ((int16_t) ExternHeightValue + (int16_t) FCParam.MaxHeight) * (int16_t)ParamSet.Height_Gain;
/beta/Code Redesign killagreg/main.c
205,7 → 205,7
ParamSet_Init();
 
// Check connected BL-Ctrls
motor_read = 0;
BLFlags |= BLFLAG_READ_VERSION;
UpdateMotor = 0;
SendMotorData();
while(!UpdateMotor);
217,8 → 217,8
UpdateMotor = 0;
SendMotorData();
while(!UpdateMotor); // wait 2 ms to finish data transfer
if(Mixer.Motor[i][MIX_GAS] > 0) // wait max 2 sec for the BL-Ctrls to wake up
{
if(Mixer.Motor[i][MIX_GAS] > 0) // ignore BLs that not are not used in the mixer table
{ // wait max 4 sec for the BL-Ctrls to wake up
while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) // while not timeout and motor is not present
{
UpdateMotor = 0;
243,10 → 243,9
Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter
}
printf("\n\r===================================");
SendMotorData();
TransmitBlConfig = 1; // enable sending of BL config
motor_read = 0;
 
I2C_SendBLConfig();
 
//wait for a short time (otherwise the RC channel check won't work below)
timer = SetDelay(500);
while(!CheckDelay(timer));
/beta/Code Redesign killagreg/makefile
16,7 → 16,7
#OPTIONS
 
# enable debug commands
DEBUG = 1
DEBUG = 0
 
# Use one of the extensions for a gps solution
EXT = NAVICTRL
285,7 → 285,7
COPY = cp
 
HEXSIZE = $(SIZE) --target=$(FORMAT) $(TARGET).hex
ELFSIZE = $(SIZE) -A $(TARGET).elf
ELFSIZE = $(SIZE) -x -A $(TARGET).elf
LIMITS = $(SIZE) -C --mcu=$(MCU) $(TARGET).elf
 
# Define Messages
/beta/Code Redesign killagreg/timer0.c
162,38 → 162,37
{
UpdateMotor = 1; // every 2nd run (976Hz/2 = 488 Hz)
}
}
 
// beeper on if duration is not over
if(BeepTime >= 10) // resolution of 1 ms second is enought
{
BeepTime -= 10; // decrement BeepTime
if(BeepTime & BeepModulation) Beeper_On = 1;
else Beeper_On = 0;
}
else // beeper off if duration is over
{
Beeper_On = 0;
BeepModulation = 0xFFFF;
}
 
// beeper on if duration is not over
if(BeepTime) // resolution of 1 ms second is enought
{
BeepTime--; // decrement BeepTime
if(BeepTime & BeepModulation) Beeper_On = 1;
else Beeper_On = 0;
#ifdef USE_BEEPER
// if beeper is on
if(Beeper_On)
{
// set speaker port to high
if(BoardRelease == 10) PORTD |= (1<<PORTD2); // Speaker at PD2
else PORTC |= (1<<PORTC7); // Speaker at PC7
}
else // beeper is off
{
// set speaker port to low
if(BoardRelease == 10) PORTD &= ~(1<<PORTD2);// Speaker at PD2
else PORTC &= ~(1<<PORTC7);// Speaker at PC7
}
#endif
}
else // beeper off if duration is over
{
Beeper_On = 0;
BeepModulation = 0xFFFF;
}
 
#ifdef USE_BEEPER
// if beeper is on
if(Beeper_On)
{
// set speaker port to high
if(BoardRelease == 10) PORTD |= (1<<PORTD2); // Speaker at PD2
else PORTC |= (1<<PORTC7); // Speaker at PC7
}
else // beeper is off
{
// set speaker port to low
if(BoardRelease == 10) PORTD &= ~(1<<PORTD2);// Speaker at PD2
else PORTC &= ~(1<<PORTC7);// Speaker at PC7
}
#endif
 
#ifndef USE_NAVICTRL
// update compass value if this option is enabled in the settings
if(ParamSet.Config0 & (CFG0_COMPASS_ACTIVE|CFG0_GPS_ACTIVE))
/beta/Code Redesign killagreg/twimaster.c
62,15 → 62,29
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint8_t TransmitBlConfig = 0;
 
volatile uint16_t I2CTimeout = 100;
 
uint8_t MissingMotor = 0;
 
uint8_t BLFlags = 0;
 
MotorData_t Motor[MAX_MOTORS];
 
BLConfig_t BLConfig =
{
MASK_SET_PWM_SCALING|MASK_SET_CURRENT_LIMIT|MASK_SET_TEMP_LIMIT|MASK_SET_CURRENT_SCALING|MASK_SET_BITCONFIG,
255, // MaxPWM
30, // Current Limit in A
99, // Temperature Limit in °C
64, // Current Scaling
0, // BitConfig
};
 
uint8_t *pTxBuff = (uint8_t*)&BLConfig;
 
#define BL_CONF_SYNC_BYTE '#'
 
#define SCL_CLOCK 200000L
#define I2C_TIMEOUT 30000
 
103,8 → 117,11
 
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;
}
 
114,6 → 131,7
/****************************************/
/* Start I2C */
/****************************************/
/*
void I2C_Start(uint8_t start_state)
{
twi_state = start_state;
127,11 → 145,12
// enable TWI Interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWSTA) | (1<<TWEN) | (1<<TWIE);
}
*/
 
/****************************************/
/* Stop I2C */
/****************************************/
void I2C_Stop(uint8_t start_state)
/*void I2C_Stop(uint8_t start_state)
{
twi_state = start_state;
// TWI Control Register
143,12 → 162,14
// enable i2c (TWEN = 1)
// disable TWI Interrupt (TWIE = 0)
TWCR = (1<<TWINT) | (1<<TWSTO) | (1<<TWEN);
}
}*/
 
 
/****************************************/
/* Write to I2C */
/****************************************/
#define I2C_WriteByte(byte) {TWDR = byte; TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);}
/*
void I2C_WriteByte(int8_t byte)
{
// move byte to send into TWI Data Register
158,25 → 179,30
// enable interrupt (TWIE = 1)
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
*/
 
 
/****************************************/
/* Receive byte and send ACK */
/****************************************/
#define I2C_ReceiveByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);}
/*
void I2C_ReceiveByte(void)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE) | (1<<TWEA);
}
*/
 
/****************************************/
/* I2C receive last byte and send no ACK*/
/****************************************/
#define I2C_ReceiveLastByte() {TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);}
/*
void I2C_ReceiveLastByte(void)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE);
}
*/
 
 
/****************************************/
/* Reset I2C */
/****************************************/
198,14 → 224,12
I2C_Start(TWI_STATE_MOTOR_TX);
}
 
 
/****************************************/
/* I2C ISR */
/****************************************/
/*
ISR (TWI_vect)
{
static uint8_t missing_motor = 0;
static uint8_t missing_motor = 0, byte_counter = 0, crc = 0, read_more = 0, motor_read_temperature = 0;
 
switch (twi_state++) // First i2c_start from SendMotorData()
{
215,157 → 239,56
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++;
if(motor_write >= MAX_MOTORS) // writing finished, read now
{
motor_write = 0;
motor_write = 0; // reset motor write counter for next cycle
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave adress in rx mode
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave address in rx mode
}
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave adress in tx mode
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave address in tx mode
break;
case 1: // Send Data to Slave
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint
break;
case 2: // repeat case 0+1 for all motors
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit setpoint
// if old version has been detected
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK))
{
if(!missing_motor) missing_motor = motor_write + 1;
if((Motor[motor_write].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[motor_write].State++; // increment error counter and handle overflow
twi_state = 4; //jump over sending more data
}
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write++; // next motor
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
else if(!(Motor[motor_write].SetPointLowerBits || (BLFlags & BLFLAG_SEND_CONFIG)) )
{ // or LowerBits are zero and no BlConfig should be sent (saves round trip time)
twi_state = 4; //jump over sending more data
}
break;
// Master Receive Data
case 3:
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received
{ // no response from the addressed slave received
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
motor_read++; // next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
case 2: // lower bits of setpoint (higher resolution)
I2C_WriteByte((Motor[motor_write].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) && !(FCFlags & FCFLAG_MOTOR_RUN) && (motor_write == motor_read))
{ // prepare sending of configuration
byte_counter = 0; // reset send byte counter
crc = 0xAA; // init checksum
}
else
{
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
I2C_ReceiveByte(); //Transmit 1st byte
else
{ // jump to state for end of transmission for that motor
twi_state = 4;
}
MissingMotor = missing_motor;
missing_motor = 0;
break;
case 4: //Read 1st byte and transmit 2nd Byte
Motor[motor_read].Current = TWDR;
I2C_ReceiveLastByte(); // nack
break;
case 5:
//Read 2nd byte
Motor[motor_read].MaxPWM = TWDR;
motor_read++; // next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
I2C_Stop(TWI_STATE_MOTOR_TX);
break;
 
// writing Gyro-Offsets
case 7:
I2C_WriteByte(0x98); // Address the DAC
break;
 
case 8:
I2C_WriteByte(0x10 + (dac_channel * 2)); // Select DAC Channel (0x10 = A, 0x12 = B, 0x14 = C)
break;
 
case 9:
switch(dac_channel)
case 3: // send configuration
if(!byte_counter) // first byte?
{
case 0:
I2C_WriteByte(DacOffsetGyroNick); // 1st byte for Channel A
break;
case 1:
I2C_WriteByte(DacOffsetGyroRoll); // 1st byte for Channel B
break;
case 2:
I2C_WriteByte(DacOffsetGyroYaw ); // 1st byte for Channel C
break;
I2C_WriteByte(BL_CONF_SYNC_BYTE);
crc += BL_CONF_SYNC_BYTE; // update crc
}
break;
 
case 10:
I2C_WriteByte(0x80); // 2nd byte for all channels is 0x80
break;
 
case 11:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
// repeat case 7...10 until all DAC Channels are updated
if(dac_channel < 2)
{
dac_channel ++; // jump to next channel
I2C_Start(TWI_STATE_GYRO_OFFSET_TX); // start transmission for next channel
}
else
{ // data to last motor send
dac_channel = 0; // reset dac channel counter
}
break;
 
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
}
}*/
 
 
/****************************************/
/* I2C ISR */
/****************************************/
ISR (TWI_vect)
{
static uint8_t missing_motor = 0, send = 0, crc = 0;
uint8_t test[] = {0,0,'#',0x1F,255,30,100,64,0x00,7,8,9,10};
 
switch (twi_state++) // First i2c_start from SendMotorData()
{
// Master Transmit
case 0: // TWI_STATE_MOTOR_TX
// skip motor if not used in mixer
while((Mixer.Motor[motor_write][MIX_GAS] <= 0) && (motor_write < MAX_MOTORS)) motor_write++;
if(motor_write >= MAX_MOTORS) // writing finished, read now
{
motor_write = 0; // reset motor write counter for next cycle
twi_state = TWI_STATE_MOTOR_RX;
I2C_WriteByte(0x53 + (motor_read * 2) ); // select slave adress in rx mode
}
else I2C_WriteByte(0x52 + (motor_write * 2) ); // select slave adress in tx mode
send = 0; // reset send byte counter
break;
case 1: // Send Data to Slave
I2C_WriteByte(Motor[motor_write].SetPoint); // transmit rotation rate setpoint
// jump over sending more data if old version has been detected
if(!(Motor[motor_write].Version & MOTOR_STATE_NEW_PROTOCOL_MASK)) twi_state = 3;
break;
case 2:
if(!send++)
{ // init
I2C_WriteByte((Motor[motor_write].SetPointLowerBits << 1)); // send the lower bits of pwm
crc = 0xAA; // init calculation of crc
}
else if(send == 9)
else if(byte_counter >= sizeof(BLConfig_t)+1) // last byte?
{ // send crc byte at the end
I2C_WriteByte(crc);
}
else
{ // transmit data to BLs
crc += test[send]; // update crc
I2C_WriteByte(test[send]); // submit next byte
}
// check for end of transmission condition
if(TransmitBlConfig && !(FCFlags&FCFLAG_MOTOR_RUN) && (motor_write == motor_read) && (Motor[motor_read].Version & MOTOR_STATE_NEW_PROTOCOL_MASK) )
else // transmit configuration to BLs
{
if(send <= 10) twi_state = 2; // repeat sending data to this motor
I2C_WriteByte(pTxBuff[byte_counter-1]); // submit next byte
crc += pTxBuff[byte_counter-1]; // update crc
twi_state = 3; // stay in this state
}
byte_counter++; // next byte
break;
case 3: // repeat case 0+1+2+3 for all motors
case 4: // repeat case 0-4 for all motors
if(TWSR == TW_MT_DATA_NACK) // Data transmitted, NACK received
{
if(!missing_motor) missing_motor = motor_write + 1;
377,50 → 300,93
I2C_Start(TWI_STATE_MOTOR_TX); // Repeated start -> switch slave or switch Master Transmit -> Master Receive
break;
// Master Receive Data
case 4: // TWI_STATE_MOTOR_RX
case 5: // TWI_STATE_MOTOR_RX
if(TWSR != TW_MR_SLA_ACK) // SLA+R transmitted, if not ACK received
{ // no response from the addressed slave received
Motor[motor_read].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit
motor_read++; // read next motor
if(motor_read >= MAX_MOTORS) motor_read = 0; // restart reading of first motor if we have reached the last one
if(++motor_read >= MAX_MOTORS)
{ // all motors read
motor_read = 0; // restart from beginning
BLFlags &= ~BLFLAG_SEND_CONFIG;
if(++motor_read_temperature >= MAX_MOTORS)
{
motor_read_temperature = 0;
BLFlags &= ~BLFLAG_READ_VERSION;
}
}
I2C_Stop(TWI_STATE_MOTOR_TX);
BLFlags |= BLFLAG_TX_COMPLETE;
}
else
{
{ // motor successfully addressed
Motor[motor_read].State |= MOTOR_STATE_PRESENT_MASK; // set present bit
I2C_ReceiveByte(); //Transmit 1st byte
// read cylclic more data
if(motor_read == motor_read_temperature)
{
read_more = 1;
I2C_ReceiveByte(); // read next byte
}
else
{
read_more = 0;
I2C_ReceiveLastByte(); //read last byte
}
}
MissingMotor = missing_motor;
missing_motor = 0;
break;
case 5: //Read 1st byte and transmit 2nd Byte
Motor[motor_read].Current = TWDR;
I2C_ReceiveByte(); // ack
case 6: // 1st byte recieved
Motor[motor_read].Current = TWDR; // get Current
if(read_more)
{
I2C_ReceiveByte(); // read 2nd byte
}
else
{
if(++motor_read >= MAX_MOTORS)
{
motor_read = 0; // restart from beginning
BLFlags &= ~BLFLAG_SEND_CONFIG;
if(++motor_read_temperature >= MAX_MOTORS)
{
motor_read_temperature = 0;
BLFlags &= ~BLFLAG_READ_VERSION;
}
}
I2C_Stop(TWI_STATE_MOTOR_TX);
BLFlags |= BLFLAG_TX_COMPLETE;
}
break;
case 6:
//Read 2nd byte and transmit 3rd Byte
Motor[motor_read].MaxPWM = TWDR;
if(TWDR == 250)
{ // a new BL found
Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
case 7: // 2nd byte recieved
Motor[motor_read].MaxPWM = TWDR; // get MaxPWM
if(BLFlags & BLFLAG_READ_VERSION)
{
if(TWDR == 250)
{ // a new BL found
if(!(FCFlags & FCFLAG_MOTOR_RUN)) Motor[motor_read].Version |= MOTOR_STATE_NEW_PROTOCOL_MASK;
}
else
{ // its not a new BL
Motor[motor_read].Version = 0;
}
}
else if(TransmitBlConfig)
{ // its not a new BL
Motor[motor_read].Version = 0; // jump over send config block in tx cycle
}
I2C_ReceiveLastByte(); // nack
break;
 
case 7:
// read 3rd byte
Motor[motor_read].Temperature = TWDR;
motor_read++; // next motor
if(motor_read >= MAX_MOTORS)
case 8: // 3rd byte received
Motor[motor_read].Temperature = TWDR; // get Temperature
if(++motor_read >= MAX_MOTORS)
{
TransmitBlConfig = 0; // send config only once
motor_read = 0; // restart reading of first motor if we have reached the last one
motor_read = 0; // restart reading of first motor
BLFlags &= ~BLFLAG_SEND_CONFIG;
if(++motor_read_temperature >= MAX_MOTORS)
{
motor_read_temperature = 0;
BLFlags &= ~BLFLAG_READ_VERSION;
}
}
I2C_Stop(TWI_STATE_MOTOR_TX);
BLFlags |= BLFLAG_TX_COMPLETE;
break;
 
// writing Gyro-Offsets
468,8 → 434,30
 
default:
I2C_Stop(TWI_STATE_MOTOR_TX);
BLFlags &= ~BLFLAG_TX_COMPLETE;
I2CTimeout = 10;
motor_write = 0;
motor_read = 0;
}
}
 
 
void I2C_SendBLConfig(void)
{
uint8_t i;
BLFlags |= BLFLAG_SEND_CONFIG; // enable sending of BL config
while(!(BLFlags & BLFLAG_TX_COMPLETE)); //wait for complete transfer
for(i = 0; i < MAX_MOTORS; i++)
{
Motor[i].SetPoint = 0;
Motor[i].SetPointLowerBits = 0;
}
motor_read = 0;
// needs at least MAX_MOTORS loops of 2 ms (12*2ms = 24ms)
do
{
I2C_Start(TWI_STATE_MOTOR_TX); // 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
}
 
/beta/Code Redesign killagreg/twimaster.h
5,14 → 5,14
#include <inttypes.h>
 
#define TWI_STATE_MOTOR_TX 0
#define TWI_STATE_MOTOR_RX 4
#define TWI_STATE_MOTOR_RX 5
#define TWI_STATE_GYRO_OFFSET_TX 18
 
extern volatile uint8_t twi_state;
extern volatile uint8_t motor_write;
extern volatile uint8_t motor_read;
extern volatile uint8_t TransmitBlConfig;
 
 
extern uint8_t MissingMotor;
 
#define MAX_MOTORS 12
21,24 → 21,62
 
#define MOTOR_STATE_NEW_PROTOCOL_MASK 0x01
 
#define BLFLAG_READ_VERSION 0x01
#define BLFLAG_SEND_CONFIG 0x02
#define BLFLAG_TX_COMPLETE 0x04
 
extern uint8_t BLFlags;
 
typedef struct
{
uint8_t Version; // the version of the BL (0 = old)
uint8_t SetPoint; // written by attitude controller
uint8_t SetPointLowerBits; // for higher Resolution of new BLs
uint8_t State; // 7 bit for I2C error counter, highest bit indicates if motor is present
uint8_t Current; // in 0.1 A steps, read back from BL
uint8_t MaxPWM; // read back from BL is less than 255 if BL is in current limit
int8_t Temperature; // old BL-Ctrl will return a 255 here
uint8_t SetPointLowerBits; // for higher Resolution
uint8_t Version;
int8_t Temperature; // old BL-Ctrl will return a 255 here, the new version the temp. in °C
} __attribute__((packed)) MotorData_t;
 
extern MotorData_t Motor[MAX_MOTORS];
 
#define MASK_SET_PWM_SCALING 0x01
#define MASK_SET_CURRENT_LIMIT 0x02
#define MASK_SET_TEMP_LIMIT 0x04
#define MASK_SET_CURRENT_SCALING 0x08
#define MASK_SET_BITCONFIG 0x10
#define MASK_RESET_CAPCOUNTER 0x20
#define MASK_SET_DEFAULT_PARAMS 0x40
#define MASK_SET_SAVE_EEPROM 0x80
 
#define BITCONF_REVERSE_ROTATION 0x01
#define BITCONF_RES1 0x02
#define BITCONF_RES2 0x04
#define BITCONF_RES3 0x08
#define BITCONF_RES4 0x10
#define BITCONF_RES5 0x20
#define BITCONF_RES6 0x40
#define BITCONF_RES7 0x80
 
typedef struct
{
uint8_t SetMask; // settings mask
uint8_t PwmScaling; // maximum value of control pwm, acts like a thrust limit
uint8_t CurrentLimit; // current limit in A
uint8_t TempLimit; // in °C
uint8_t CurrentScaling; // scaling factor for current measurement
uint8_t BitConfig; // see defines below
} __attribute__((packed)) BLConfig_t;
 
extern BLConfig_t BLConfig;
 
 
extern volatile uint16_t I2CTimeout;
 
extern void I2C_Init (void); // Initialize I2C
extern void I2C_Start(uint8_t start_state); // Start I2C
extern void I2C_Stop (uint8_t start_state); // Stop I2C
extern void I2C_Reset(void); // Reset I2C
void I2C_Init (void); // Initialize I2C
#define I2C_Start(start_state) {twi_state = start_state; BLFlags &= ~BLFLAG_TX_COMPLETE; TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE);}
#define I2C_Stop(start_state) {twi_state = start_state; TWCR = (1<<TWEN) | (1<<TWSTO) | (1<<TWINT);}
void I2C_Reset(void); // Reset I2C
void I2C_SendBLConfig(void);
 
#endif
/beta/Code Redesign killagreg/uart0.c
69,10 → 69,9
#include "mk3mag.h"
#endif
#include "libfc.h"
#ifdef DEBUG
#include "debug.h"
#endif
 
 
#define FC_ADDRESS 1
#define NC_ADDRESS 2
#define MK3MAG_ADDRESS 3