/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 |