Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 942 → Rev 943

/branches/V0.70d CRK HexaLotte/fc.c
128,7 → 128,11
int16_t Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
 
// setpoints for motors
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left;
#ifdef HEXAKOPTER
volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left;
#else
volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
#endif
 
// stick values derived by rc channels readings
int16_t StickNick = 0, StickRoll = 0, StickYaw = 0, StickGas = 0;
382,6 → 386,18
{
if(!(MKFlags & MKFLAG_MOTOR_RUN))
{
#ifdef HEXAKOPTER
Motor_RearLeft = 0;
Motor_FrontLeft = 0;
Motor_RearRight = 0;
Motor_FrontRight = 0;
Motor_Right = 0;
Motor_Left = 0;
if(MotorTest[0]) Motor_FrontLeft = Motor_FrontRight = MotorTest[0];
if(MotorTest[1]) Motor_RearLeft = Motor_RearRight = MotorTest[1];
if(MotorTest[2]) Motor_Left = MotorTest[2];
if(MotorTest[3]) Motor_Right = MotorTest[3];
#else
Motor_Rear = 0;
Motor_Front = 0;
Motor_Right = 0;
390,13 → 406,10
if(MotorTest[1]) Motor_Rear = MotorTest[1];
if(MotorTest[2]) Motor_Left = MotorTest[2];
if(MotorTest[3]) Motor_Right = MotorTest[3];
#endif
MKFlags &= ~(MKFLAG_FLY|MKFLAG_START); // clear flag FLY and START if motors are off
}
DebugOut.Analog[12] = Motor_Front;
DebugOut.Analog[13] = Motor_Rear;
DebugOut.Analog[14] = Motor_Left;
DebugOut.Analog[15] = Motor_Right;
 
//Start I2C Interrupt Mode
twi_state = TWI_STATE_MOTOR_TX;
I2C_Start();
474,7 → 487,7
void MotorControl(void)
{
int16_t MotorValue, pd_result, h, tmp_int;
int16_t YawMixFraction, GasMixFraction;
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction;
static int32_t SumNick = 0, SumRoll = 0;
static int32_t SetPointYaw = 0;
static int32_t IntegralErrorNick = 0;
575,6 → 588,7
if(Poti7 < 0) Poti7 = 0; else if(Poti7 > 255) Poti7 = 255;
if(Poti8 < 0) Poti8 = 0; else if(Poti8 > 255) Poti8 = 255;
 
 
// if motors are off and the gas stick is in the upper position
if((PPM_in[ParamSet.ChannelAssignment[CH_GAS]] > 80) && !(MKFlags & MKFLAG_MOTOR_RUN) )
{
694,7 → 708,7
{
if(++delay_startmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_startmotors = 200; // do not repeat if once executed
delay_startmotors = 0; // do not repeat if once executed
Model_Is_Flying = 1;
MKFlags |= (MKFLAG_MOTOR_RUN|MKFLAG_START); // set flag RUN and START
SetPointYaw = 0;
715,7 → 729,7
{
if(++delay_stopmotors > 200) // not immediately (wait 200 loops = 200 * 2ms = 0.4 s)
{
delay_stopmotors = 200; // do not repeat if once executed
delay_stopmotors = 0; // do not repeat if once executed
Model_Is_Flying = 0;
MKFlags &= ~(MKFLAG_MOTOR_RUN);
}
1231,12 → 1245,28
DebugOut.Analog[4] = Reading_GyroYaw;
DebugOut.Analog[5] = ReadingHeight;
DebugOut.Analog[6] = (Reading_Integral_Top / 512);
DebugOut.Analog[8] = CompassHeading;
// DebugOut.Analog[8] = CompassHeading;
DebugOut.Analog[8] = RC_Quality;
DebugOut.Analog[9] = UBat;
DebugOut.Analog[10] = RC_Quality;
 
#ifdef HEXAKOPTER
DebugOut.Analog[10] = Motor_FrontLeft;
DebugOut.Analog[11] = Motor_FrontRight;
DebugOut.Analog[12] = Motor_RearLeft;
DebugOut.Analog[13] = Motor_RearRight;
DebugOut.Analog[14] = Motor_Left;
DebugOut.Analog[15] = Motor_Right;
#else
DebugOut.Analog[11] = YawGyroHeading / YAW_GYRO_DEG_FACTOR;
//DebugOut.Analog[16] = Mean_AccTop;
 
DebugOut.Analog[12] = Motor_Front;
DebugOut.Analog[13] = Motor_Rear;
DebugOut.Analog[14] = Motor_Left;
DebugOut.Analog[15] = Motor_Right;
#endif
 
DebugOut.Analog[16] = Mean_AccTop;
 
DebugOut.Analog[20] = ServoValue;
 
 
1388,21 → 1418,8
if(pd_result > tmp_int) pd_result = tmp_int;
if(pd_result < -tmp_int) pd_result = -tmp_int;
 
// Motor Front
MotorValue = GasMixFraction + pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
NickMixFraction = pd_result;
 
// Motor Rear
MotorValue = GasMixFraction - pd_result + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Roll-Axis
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1416,20 → 1433,102
if(pd_result > tmp_int) pd_result = tmp_int;
if(pd_result < -tmp_int) pd_result = -tmp_int;
 
RollMixFraction = pd_result;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Calculate Motor Mixes
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
#ifdef HEXAKOPTER
// Motor FrontLeft
MotorValue = GasMixFraction
+ NickMixFraction
+ RollMixFraction/2
- YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_FrontLeft = MotorValue;
 
// Motor FrontRight
MotorValue = GasMixFraction
+ NickMixFraction
- RollMixFraction/2
+ YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_FrontRight = MotorValue;
 
// Motor RearLeft
MotorValue = GasMixFraction
- NickMixFraction
+ RollMixFraction/2
- YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_RearLeft = MotorValue;
 
// Motor RearRight
MotorValue = GasMixFraction
- NickMixFraction
- RollMixFraction/2
+ YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_RearRight= MotorValue;
 
// Motor Left
MotorValue = GasMixFraction + pd_result - YawMixFraction; // Mixer
MotorValue = GasMixFraction
+ RollMixFraction
+ YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Left = MotorValue;
 
// Motor Right
MotorValue = GasMixFraction - pd_result - YawMixFraction; // Mixer
// Motor Right
MotorValue = GasMixFraction
- RollMixFraction
- YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Right = MotorValue;
 
#else
 
// Motor Front
MotorValue = GasMixFraction + NickMixFraction + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Front = MotorValue;
 
// Motor Rear
MotorValue = GasMixFraction - NickMixFraction + YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if ((MotorValue < 0)) MotorValue = 0;
else if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Rear = MotorValue;
 
 
// Motor Left
MotorValue = GasMixFraction + RollMixFraction - YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Left = MotorValue;
 
 
// Motor Right
MotorValue = GasMixFraction - RollMixFraction - YawMixFraction; // Mixer
MotorValue /= STICK_GAIN;
if(MotorValue > ParamSet.Gas_Max) MotorValue = ParamSet.Gas_Max;
else if (MotorValue < ParamSet.Gas_Min) MotorValue = ParamSet.Gas_Min;
Motor_Right = MotorValue;
#endif
}
 
/branches/V0.70d CRK HexaLotte/fc.h
97,7 → 97,11
extern int16_t Poti1, Poti2, Poti3, Poti4, Poti5, Poti6, Poti7, Poti8;
 
// setpoints for motors
#ifdef HEXAKOPTER
extern volatile uint8_t Motor_FrontLeft, Motor_FrontRight, Motor_RearLeft, Motor_RearRight, Motor_Right, Motor_Left;
#else
extern volatile uint8_t Motor_Front, Motor_Rear, Motor_Right, Motor_Left; //used by twimaster isr
#endif
 
// current stick values
extern int16_t StickNick;
/branches/V0.70d CRK HexaLotte/main.c
230,7 → 230,7
 
LCD_Clear();
 
I2CTimeout = 5000;
I2CTimeout = 2000;
 
while (1)
{
/branches/V0.70d CRK HexaLotte/main.h
14,6 → 14,11
 
#define F_CPU SYSCLK
 
#ifdef HEXAKOPTER
#define MOTOR_COUNT 6
#else
#define MOTOR_COUNT 4
#endif
 
// neue Hardware
#define RED_OFF {if(BoardRelease == 10) PORTB &=~(1<<PORTB0); else PORTB |= (1<<PORTB0);}
/branches/V0.70d CRK HexaLotte/makefile
3,7 → 3,12
#MCU = atmega644
MCU = atmega644p
F_CPU = 20000000
#
#-------------------------------------------------------------------
# comment out for Quadro, uncomment for Hexakopter
# HEXAKOPTER = _HEXA
#-------------------------------------------------------------------
 
VERSION_MAJOR = 0
VERSION_MINOR = 70
VERSION_INDEX = 3
12,9 → 17,10
#-------------------------------------------------------------------
#OPTIONS
# Use one of the extensions for a gps solution
EXT = KILLAGREG
#EXT = KILLAGREG
#EXT = NAVICTRL
#EXT = MK3MAG
EXT = MK3MAG
 
#-------------------------------------------------------------------
 
ifeq ($(MCU), atmega644)
21,12 → 27,12
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
#FUSE_SETTINGS = -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
# -u bei neuen Controllern wieder einspielen
HEX_NAME = MEGA644_$(EXT)
HEX_NAME = MEGA644$(HEXAKOPTER)_$(EXT)
endif
 
ifeq ($(MCU), atmega644p)
FUSE_SETTINGS = -u -U lfuse:w:0xff:m -U hfuse:w:0xdf:m
HEX_NAME = MEGA644p_$(EXT)
HEX_NAME = MEGA644p$(HEXAKOPTER)_$(EXT)
endif
 
 
152,9 → 158,11
CFLAGS += -DUSE_NAVICTRL
endif
 
ifeq ($(HEXAKOPTER), _HEXA)
CFLAGS += -DHEXAKOPTER
endif
 
 
 
# Optional assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create listing
/branches/V0.70d CRK HexaLotte/twimaster.c
13,7 → 13,7
volatile uint8_t motor_write = 0;
volatile uint8_t motor_read = 0;
volatile uint8_t dac_channel = 0;
volatile uint8_t motor_rx[8];
volatile uint8_t motor_rx[MOTOR_COUNT*2];
volatile uint16_t I2CTimeout = 100;
 
 
161,9 → 161,33
I2C_WriteByte(0x52 + (motor_write * 2) );
break;
case 1: // Send Data to Slave
#ifdef HEXAKOPTER
switch(motor_write)
{
{
case 0:
I2C_WriteByte(Motor_FrontLeft);
break;
case 1:
I2C_WriteByte(Motor_RearRight);
break;
case 2:
I2C_WriteByte(Motor_FrontRight);
break;
case 3:
I2C_WriteByte(Motor_RearLeft);
break;
case 4:
I2C_WriteByte(Motor_Right);
break;
case 5:
I2C_WriteByte(Motor_Left);
break;
}
 
#else
switch(motor_write++)
{
case 0:
I2C_WriteByte(Motor_Front);
break;
case 1:
176,10 → 200,11
I2C_WriteByte(Motor_Left);
break;
}
#endif
break;
case 2: // repeat case 0+1 for all motors
I2C_Stop();
if (motor_write < 3)
I2C_Stop();
if (motor_write < (MOTOR_COUNT-1))
{
motor_write++; // jump to next motor
twi_state = 0; // and repeat from state 0
190,7 → 215,6
}
I2C_Start(); // Repeated start -> switch salve or switch Master Transmit -> Master Receive
break;
 
// Master Receive
case 3: // Send SLA-R
I2C_WriteByte(0x53 + (motor_read * 2) );
207,7 → 231,7
//Read 2nd byte
motor_rx[motor_read + 4] = TWDR;
motor_read++;
if (motor_read > 3) motor_read = 0;
if (motor_read > (MOTOR_COUNT-1)) motor_read = 0;
I2C_Stop();
twi_state = 0;
I2CTimeout = 10;
/branches/V0.70d CRK HexaLotte/twimaster.h
9,7 → 9,7
#define TWI_STATE_GYRO_OFFSET_TX 7
 
extern volatile uint8_t twi_state;
extern volatile uint8_t motor_rx[8];
extern volatile uint8_t motor_rx[2*MOTOR_COUNT];
extern volatile uint16_t I2CTimeout;
 
extern void I2C_Init (void); // Initialize I2C
/branches/V0.70d CRK HexaLotte/uart.c
73,15 → 73,24
"ReadingHeight ", //5
"AccZ ",
"Gas ",
"CompassHeading ",
"Receiver Level ",
"Voltage ",
"Receiver Level ", //10
"YawGyroHeading ",
"Motor_Front ",
"Motor_Rear ",
#ifdef HEXAKOPTER
"Motor FrontLeft ", //10
"Motor FrontRight",
"Motor RearLeft ",
"Motor RearRight ",
"Motor Right ",
"Motor Left ", //15
#else
"Motor FrontLeft ", //10
"Motor FrontRight",
"Motor_RearLeft ",
"Motor_RearRight ",
"Motor_Right ",
"Motor_Left ", //15
" ",
#endif
"Acc_Z ",
"SPI Error ",
"SPI Ok ",
" ",
/branches/V0.70d CRK HexaLotte/version.txt
178,4 → 178,9
- Zusätzliche Punkte im Menü des KopterTool zur Anzeige des GPS-Status und der MM3-Kalibierparameter
 
Anpassungen für Hexakopter
S.Eisenbeiss (pangu) 11.08.2008
 
- Makefile: define HEXAKOPTER to compile for x-type Hexakopter
- main.h, fc.*, twimaster.*, uart.c, : added motorcalculations, debugout for Hexakopter