/trunk/fc.c |
---|
157,8 → 157,6 |
volatile unsigned char FCFlags = 0; |
long GIER_GRAD_FAKTOR = 1291; |
signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
unsigned char RequiredMotors = 4; |
unsigned char Motor[MAX_MOTORS]; |
signed int tmp_motorwert[MAX_MOTORS]; |
unsigned char LoadHandler = 0; |
#define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
481,16 → 479,16 |
for(i=0;i<MAX_MOTORS;i++) |
{ |
if(!PC_MotortestActive) MotorTest[i] = 0; |
Motor[i] = MotorTest[i]; |
Motor[i].SetPoint = MotorTest[i]; |
} |
if(PC_MotortestActive) PC_MotortestActive--; |
} |
else FCFlags |= FCFLAG_MOTOR_RUN; |
DebugOut.Analog[12] = Motor[0]; |
DebugOut.Analog[13] = Motor[1]; |
DebugOut.Analog[14] = Motor[3]; |
DebugOut.Analog[15] = Motor[2]; |
DebugOut.Analog[12] = Motor[0].SetPoint; |
DebugOut.Analog[13] = Motor[1].SetPoint; |
DebugOut.Analog[14] = Motor[3].SetPoint; |
DebugOut.Analog[15] = Motor[2].SetPoint; |
//Start I2C Interrupt Mode |
twi_state = 0; |
1603,14 → 1601,8 |
tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
tmp_int = tmp_motorwert[i] / STICK_GAIN; |
CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
Motor[i] = tmp_int; |
Motor[i].SetPoint = tmp_int; |
} |
else Motor[i] = 0; |
else Motor[i].SetPoint = 0; |
} |
/* |
if(Poti1 > 20) Motor1 = 0; |
if(Poti1 > 90) Motor6 = 0; |
if(Poti1 > 140) Motor2 = 0; |
//if(Poti1 > 200) Motor7 = 0; |
*/ |
} |
/trunk/fc.h |
---|
27,7 → 27,6 |
#define Poti7 Poti[6] |
#define Poti8 Poti[7] |
#define MAX_MOTORS 12 |
#define CHECK_MIN_MAX(wert,min,max) {if(wert < min) wert = min; else if(wert > max) wert = max;} |
78,7 → 77,6 |
//extern int Poti1, Poti2, Poti3, Poti4; |
extern volatile unsigned char SenderOkay; |
extern volatile unsigned char SenderRSSI; |
extern unsigned char RequiredMotors; |
extern int StickNick,StickRoll,StickGier; |
extern char MotorenEin; |
extern void DefaultKonstanten1(void); |
211,6 → 209,5 |
extern unsigned char Parameter_J17Bitmask; // for the J17 Output |
extern unsigned char Parameter_J17Timing; // for the J17 Output |
extern signed char MixerTable[MAX_MOTORS][4]; |
extern unsigned char Motor[MAX_MOTORS]; |
#endif //_FC_H |
/trunk/main.c |
---|
161,6 → 161,8 |
//############################################################################ |
{ |
unsigned int timer,i,timer2 = 0, timerPolling; |
unsigned char RequiredMotors = 0; |
DDRB = 0x00; |
PORTB = 0x00; |
for(timer = 0; timer < 1000; timer++); // verzögern |
191,7 → 193,7 |
StickGier = 0; PPM_in[K_GAS] = 0;StickRoll = 0; StickNick = 0; |
if(PlatinenVersion >= 20) GIER_GRAD_FAKTOR = 1160; else GIER_GRAD_FAKTOR = 1291; // unterschiedlich für ME und ENC |
ROT_OFF; |
Timer_Init(); |
TIMER2_Init(); |
UART_Init(); |
210,7 → 212,6 |
(eeprom_read_byte(&EEPromArray[EEPROM_ADR_VALID]) != 0xff)) // Settings reset via Koptertool |
{ |
unsigned char i; |
RequiredMotors = 0; |
eeprom_read_block(&Mixer, &EEPromArray[EEPROM_ADR_MIXER_TABLE], sizeof(Mixer)); |
for(i=0; i<16;i++) { if(Mixer.Motor[i][0] > 0) RequiredMotors++;} |
} |
247,18 → 248,18 |
while(!UpdateMotor); |
if(Mixer.Motor[i][0] > 0) // wait max 2 sec for the BL-Ctrls to wake up |
{ |
while(!CheckDelay(timer) && !MotorPresent[i]) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);}; |
while(!CheckDelay(timer) && !(Motor[i].State & MOTOR_STATE_PRESENT_MASK) ) {UpdateMotor = 0; SendMotorData(); while(!UpdateMotor);}; |
} |
if(MotorPresent[i]) printf("%d ",i+1); |
if(Motor[i].State & MOTOR_STATE_PRESENT_MASK) printf("%d ",i+1); |
} |
for(i=0; i < MAX_MOTORS; i++) |
{ |
if(!MotorPresent[i] && Mixer.Motor[i][0] > 0) |
if(!(Motor[i].State & MOTOR_STATE_PRESENT_MASK) && Mixer.Motor[i][0] > 0) |
{ |
printf("\n\r\n\r!! MISSING BL-CTRL: %d !!",i+1); |
ServoActive = 1; // just in case the FC would be used as camera-stabilizer |
} |
MotorError[i] = 0; |
Motor[i].State &= ~MOTOR_STATE_ERROR_MASK; // clear error counter |
} |
printf("\n\r==================================="); |
SendMotorData(); |
/trunk/menu.c |
---|
7,9 → 7,6 |
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
#include "main.h" |
unsigned int TestInt = 0; |
#define ARRAYGROESSE 10 |
unsigned char Array[ARRAYGROESSE] = {1,2,3,4,5,6,7,8,9,10}; |
char DisplayBuff[80] = "Hallo Welt"; |
unsigned char DispPtr = 0; |
55,9 → 52,6 |
if(I2CTimeout < 6) LCD_printfxy(0,3,"I2C ERROR!!!") |
else |
if(MissingMotor) LCD_printfxy(0,3,"Missing BL-Ctrl:%d!!",MissingMotor) |
else LCD_printfxy(0,3,"(c) Holger Buss"); |
// if(RemoteTasten & KEY3) TestInt--; |
// if(RemoteTasten & KEY4) TestInt++; |
break; |
case 1: |
if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) |
161,18 → 155,18 |
break; |
case 13: |
LCD_printfxy(0,0,"BL-Ctrl Errors " ); |
LCD_printfxy(0,1,"%3d %3d %3d %3d ",MotorError[0],MotorError[1],MotorError[2],MotorError[3]); |
LCD_printfxy(0,2,"%3d %3d %3d %3d ",MotorError[4],MotorError[5],MotorError[6],MotorError[7]); |
LCD_printfxy(0,3,"%3d %3d %3d %3d ",MotorError[8],MotorError[9],MotorError[10],MotorError[11]); |
LCD_printfxy(0,1,"%3d %3d %3d %3d ",Motor[0].State & MOTOR_STATE_ERROR_MASK,Motor[1].State & MOTOR_STATE_ERROR_MASK,Motor[2].State & MOTOR_STATE_ERROR_MASK,Motor[3].State & MOTOR_STATE_ERROR_MASK); |
LCD_printfxy(0,2,"%3d %3d %3d %3d ",Motor[4].State & MOTOR_STATE_ERROR_MASK,Motor[5].State & MOTOR_STATE_ERROR_MASK,Motor[6].State & MOTOR_STATE_ERROR_MASK,Motor[7].State & MOTOR_STATE_ERROR_MASK); |
LCD_printfxy(0,3,"%3d %3d %3d %3d ",Motor[8].State & MOTOR_STATE_ERROR_MASK,Motor[9].State & MOTOR_STATE_ERROR_MASK,Motor[10].State & MOTOR_STATE_ERROR_MASK,Motor[11].State & MOTOR_STATE_ERROR_MASK); |
break; |
case 14: |
LCD_printfxy(0,0,"BL-Ctrl found " ); |
LCD_printfxy(0,1," %c %c %c %c ",MotorPresent[0] + '-',MotorPresent[1] + '-',MotorPresent[2] + '-',MotorPresent[3] + '-'); |
LCD_printfxy(0,2," %c %c %c %c ",MotorPresent[4] + '-',MotorPresent[5] + '-',MotorPresent[6] + '-',MotorPresent[7] + '-'); |
LCD_printfxy(0,3," %c - - -",MotorPresent[8] + '-'); |
if(MotorPresent[9]) LCD_printfxy(4,3,"10"); |
if(MotorPresent[10]) LCD_printfxy(8,3,"11"); |
if(MotorPresent[11]) LCD_printfxy(12,3,"12"); |
LCD_printfxy(0,1," %c %c %c %c ",'-' + 4 * (Motor[0].State>>7),'-' + 5 * (Motor[1].State>>7),'-' + 6 * (Motor[2].State>>7),'-' + 7 * (Motor[3].State>>7)); |
LCD_printfxy(0,2," %c %c %c %c ",'-' + 8 * (Motor[4].State>>7),'-' + 9 * (Motor[5].State>>7),'-' + 10 * (Motor[6].State>>7),'-' + 11 * (Motor[7].State>>7)); |
LCD_printfxy(0,3," %c - - - ",'-' + 12 * (Motor[8].State>>7)); |
if(Motor[9].State>>7) LCD_printfxy(4,3,"10"); |
if(Motor[10].State>>7) LCD_printfxy(8,3,"11"); |
if(Motor[11].State>>7) LCD_printfxy(12,3,"12"); |
break; |
case 15: |
LCD_printfxy(0,0,"Flight-Time " ); |
/trunk/twimaster.c |
---|
6,9 → 6,9 |
volatile unsigned char twi_state = 0; |
unsigned char motor = 0; |
unsigned char motorread = 0,MissingMotor = 0; |
unsigned char motor_rx[16],motor_rx2[16]; |
unsigned char MotorPresent[MAX_MOTORS]; |
unsigned char MotorError[MAX_MOTORS]; |
MotorData_t Motor[MAX_MOTORS]; |
unsigned int I2CError = 0; |
//############################################################################ |
17,12 → 17,12 |
//############################################################################ |
{ |
TWSR = 0; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
TWBR = ((SYSCLK/SCL_CLOCK)-16)/2; |
} |
//############################################################################ |
//Start I2C |
void i2c_start(void) |
void i2c_start(void) |
//############################################################################ |
{ |
TWCR = (1<<TWSTA) | (1<<TWEN) | (1<<TWINT) | (1<<TWIE); |
38,7 → 38,7 |
void i2c_reset(void) |
//############################################################################ |
{ |
i2c_stop(); |
i2c_stop(); |
twi_state = 0; |
motor = TWDR; |
motor = 0; |
56,7 → 56,7 |
//############################################################################ |
void i2c_write_byte(char byte) |
//############################################################################ |
{ |
{ |
TWSR = 0x00; |
TWDR = byte; |
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWIE); |
105,28 → 105,28 |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
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; |
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++]); |
i2c_write_byte(Motor[motor++].SetPoint); |
break; |
case 2: |
if(TWSR == 0x30) |
{ |
if(!missing_motor) missing_motor = motor; |
if(++MotorError[motor-1] == 0) MotorError[motor-1] = 255; |
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; |
i2c_start(); |
break; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Reading Data |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
133,16 → 133,16 |
case 3: |
//Transmit 1st byte for reading |
if(TWSR != 0x40) // Error? |
{ |
MotorPresent[motorread] = 0; |
motorread++; |
{ |
Motor[motorread].State &= ~MOTOR_STATE_PRESENT_MASK; // clear present bit |
motorread++; |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
} |
else |
else |
{ |
MotorPresent[motorread] = ('1' - '-') + motorread; |
Motor[motorread].State |= MOTOR_STATE_PRESENT_MASK; // set present bit |
I2C_ReceiveByte(); |
} |
MissingMotor = missing_motor; |
149,73 → 149,74 |
missing_motor = 0; |
break; |
case 4: //Read 1st byte and transmit 2nd Byte |
motor_rx[motorread] = TWDR; |
Motor[motorread].Current = TWDR; |
I2C_ReceiveLastByte(); //nack |
break; |
case 5: |
//Read 2nd byte |
motor_rx2[motorread++] = TWDR; |
Motor[motorread].MaxPWM = TWDR; |
motorread++; // next motor |
if(motorread >= MAX_MOTORS) motorread = 0; |
i2c_stop(); |
twi_state = 0; |
break; |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// writing Gyro-Offset |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
case 8: |
case 8: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 9: |
case 9: |
i2c_write_byte(0x10); // Update Channel A |
break; |
case 10: |
case 10: |
i2c_write_byte(AnalogOffsetNick); // Value |
break; |
case 11: |
case 11: |
i2c_write_byte(0x80); // Value |
break; |
case 12: |
i2c_stop(); |
case 12: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
i2c_start(); |
break; |
case 13: |
case 13: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 14: |
case 14: |
i2c_write_byte(0x12); // Update Channel B |
break; |
case 15: |
case 15: |
i2c_write_byte(AnalogOffsetRoll); // Value |
break; |
case 16: |
case 16: |
i2c_write_byte(0x80); // Value |
break; |
case 17: |
i2c_stop(); |
case 17: |
i2c_stop(); |
I2CTimeout = 10; |
i2c_start(); |
i2c_start(); |
break; |
case 18: |
case 18: |
i2c_write_byte(0x98); // Address of the DAC |
break; |
case 19: |
case 19: |
i2c_write_byte(0x14); // Update Channel C |
break; |
case 20: |
case 20: |
i2c_write_byte(AnalogOffsetGier); // Value |
break; |
case 21: |
case 21: |
i2c_write_byte(0x80); // Value |
break; |
case 22: |
i2c_stop(); |
case 22: |
i2c_stop(); |
I2CTimeout = 10; |
twi_state = 0; |
break; |
default: twi_state = 0; |
break; |
break; |
} |
TWCR |= 0x80; |
} |
/trunk/twimaster.h |
---|
21,10 → 21,21 |
extern volatile unsigned char twi_state; |
extern unsigned char motor,MissingMotor; |
extern unsigned char motorread; |
extern unsigned char motor_rx[]; |
extern unsigned char MotorPresent[]; |
extern unsigned char MotorError[]; |
#define MAX_MOTORS 12 |
#define MOTOR_STATE_PRESENT_MASK 0x80 |
#define MOTOR_STATE_ERROR_MASK 0x7F |
typedef struct |
{ |
uint8_t SetPoint; // written by attitude controller |
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 |
} __attribute__((packed)) MotorData_t; |
extern MotorData_t Motor[MAX_MOTORS]; |
void i2c_reset(void); |
extern void i2c_init (void); // I2C initialisieren |
extern void i2c_start (void); // Start I2C |