Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1478 → Rev 1479

/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