Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1329 → Rev 1330

/trunk/fc.c
56,6 → 56,7
#include "main.h"
#include "eeprom.c"
#include "mymath.h"
#include "isqrt.h"
 
unsigned char h,m,s,BaroExpandActive = 0;
volatile unsigned int I2CTimeout = 100;
542,7 → 543,7
void MotorRegler(void)
//############################################################################
{
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int;
int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
int GierMischanteil,GasMischanteil;
static long SummeNick=0,SummeRoll=0;
static long sollGier = 0,tmp_long,tmp_long2;
564,7 → 565,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
GasMischanteil = StickGas;
if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Empfang schlecht
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
650,7 → 651,7
{
if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
}
ServoActive = 0;
ServoActive = 0;
SetNeutral();
ServoActive = 1;
DDRD |=0x80; // enable J7 -> Servo signal
746,7 → 747,7
if(StickGier < -2) StickGier += 2; else StickGier = 0;
 
StickGas = (StickGas + PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120) / 2;
 
GyroFaktor = (Parameter_Gyro_P + 10.0);
IntegralFaktor = Parameter_Gyro_I;
GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0);
764,10 → 765,10
if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
}
if(StickGas < 0) StickGas = 0;
 
if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
if(GyroFaktor < 0) GyroFaktor = 0;
if(IntegralFaktor < 0) IntegralFaktor = 0;
//if(GyroFaktor < 0) GyroFaktor = 0;
//if(IntegralFaktor < 0) IntegralFaktor = 0;
 
if(abs(StickNick/STICK_GAIN) > MaxStickNick)
{
1234,8 → 1235,7
static unsigned long HooverGasFilter = 0;
static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
int CosAttitude; // for projection of hoover gas
long tmp_long3;
// const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255};
 
// get the current hooverpoint
if(LoadHandler == 1)
{
1248,35 → 1248,35
{
if(MessLuftdruck > 920)
{ // increase offset
if(OCR0A < (255 - OPA_OFFSET_STEP))
if(OCR0A < (255 - OPA_OFFSET_STEP))
{
ExpandBaro -= 1;
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
beeptime = 300;
BaroExpandActive = 50;
}
else
}
else
{
BaroAtLowerLimit = 1;
}
}
// measurement of air pressure close to lower limit and
else
else
if(MessLuftdruck < 100)
{ // decrease offset
if(OCR0A > OPA_OFFSET_STEP)
if(OCR0A > OPA_OFFSET_STEP)
{
ExpandBaro += 1;
OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
beeptime = 300;
BaroExpandActive = 50;
}
else
}
else
{
BaroAtUpperLimit = 1;
}
}
else
else
{
BaroAtUpperLimit = 0;
BaroAtLowerLimit = 0;
1315,12 → 1315,10
}
 
// calculate cos of nick and roll angle used for projection of the vertical hoover gas
J4Low;
tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg
tmp_long *= tmp_long;
tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg
tmp_long2 *= tmp_long2;
CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg
J4Low;
tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg
tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
DebugOut.Analog[16] = CosAttitude;
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude
1331,7 → 1329,7
#define HEIGHT_TRIM_DOWN 0x02
static unsigned char HeightTrimmingFlag = 0x00;
 
#define HEIGHT_CONTROL_STICKTHRESHOLD 15
#define HEIGHT_CONTROL_STICKTHRESHOLD 15
// Holger original version
// start of height control algorithm
// the height control is only an attenuation of the actual gas stick.
1388,13 → 1386,13
if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75;
//update hoover gas stick value when setpoint is shifted
// StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value
// StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
// if(StickGasHoover < 70) StickGasHoover = 70;
// else if(StickGasHoover > 150) StickGasHoover = 150;
// StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
// if(StickGasHoover < 70) StickGasHoover = 70;
// else if(StickGasHoover > 150) StickGasHoover = 150;
}
} //if MikroKopterFlags & MKFLAG_FLY
else
{
else
{
SollHoehe = HoehenWert - 400;
if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
else StickGasHoover = 120;
1420,7 → 1418,7
tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// limit deviation from hoover point within the target region
if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero
{
1432,13 → 1430,13
tmp_int = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L;
LIMIT_MIN_MAX(tmp_int, -127, 255);
HCGas -= tmp_int;
 
// strech control output by inverse attitude projection 1/cos
// + 1/cos(angle) ++++++++++++++++++++++++++
tmp_long3 = (int32_t)HCGas;
tmp_long3 *= 8192L;
tmp_long3 /= CosAttitude;
HCGas = (int16_t)tmp_long3;
tmp_long2 = (int32_t)HCGas;
tmp_long2 *= 8192L;
tmp_long2 /= CosAttitude;
HCGas = (int16_t)tmp_long2;
// update height control gas averaging
FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
// limit height control gas pd-control output
1460,10 → 1458,10
StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning;
}
else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint;
if(StickGasHoover < 70) StickGasHoover = 70;
else if(StickGasHoover > 150) StickGasHoover = 150;
if(StickGasHoover < 70) StickGasHoover = 70;
else if(StickGasHoover > 150) StickGasHoover = 150;
FilterHCGas = GasMischanteil;
}
}
 
// Hoover gas estimation by averaging gas control output on small z-velocities
// this is done only if height contol option is selected in global config and aircraft is flying
1472,30 → 1470,30
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
if(abs(VarioMeter) < 100) // only on small vertical speed
{
tmp_long3 = (int32_t)GasMischanteil; // take current thrust
tmp_long3 *= CosAttitude; // apply attitude projection
tmp_long3 /= 8192;
tmp_long2 = (int32_t)GasMischanteil; // take current thrust
tmp_long2 *= CosAttitude; // apply attitude projection
tmp_long2 /= 8192;
 
// average vertical projected thrust
if(modell_fliegt < 2000) // the first 4 seconds
{ // reduce the time constant of averaging by factor of 8 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/8L);
HooverGasFilter += 8L * tmp_long3;
HooverGasFilter += 8L * tmp_long2;
}
else if(modell_fliegt < 4000) // the first 8 seconds
{ // reduce the time constant of averaging by factor of 4 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/4L);
HooverGasFilter += 4L * tmp_long3;
HooverGasFilter += 4L * tmp_long2;
}
else if(modell_fliegt < 8000) // the first 16 seconds
{ // reduce the time constant of averaging by factor of 2 to get much faster a stable value
HooverGasFilter -= HooverGasFilter/(HOOVER_GAS_AVERAGE/2L);
HooverGasFilter += 2L * tmp_long3;
HooverGasFilter += 2L * tmp_long2;
}
else //later
{
HooverGasFilter -= HooverGasFilter/HOOVER_GAS_AVERAGE;
HooverGasFilter += tmp_long3;
HooverGasFilter += tmp_long2;
}
HooverGas = (int16_t)(HooverGasFilter/HOOVER_GAS_AVERAGE);
if(EE_Parameter.Hoehe_HoverBand)
1513,11 → 1511,11
}
}
LastHCGas = GasMischanteil;
}
else
}
else
{
GasMischanteil = LastHCGas;
}
}
//DebugOut.Analog[26] = HooverGasMax;
}// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
J4Low;
1529,7 → 1527,7
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// all BL-Ctrl connected?
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(MissingMotor)
if(MissingMotor)
if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
{
modell_fliegt = 1;
/trunk/isqrt.S
0,0 → 1,203
;-----------------------------------------------------------------------------;
; Fast integer squareroot routines for avr-gcc project (C)ChaN, 2008
; http://elm-chan.org/docs/avrlib/sqrt32.S
;-----------------------------------------------------------------------------;
; uint16_t isqrt32 (uint32_t n);
; uint8_t isqrt16 (uint16_t n);
; uint16_t ihypot (int16_t x, int16_t y);
 
;-----------------------------------------------------------------------------:
; 32bit integer squareroot
;-----------------------------------------------------------------------------;
; uint16_t isqrt32 (
; uint32_t n
; );
;
; Return Value:
; Squareroot of n.
;
; Size = 53 words
; Clock = 532..548 cycles
; Stack = 0 byte
 
.global isqrt32
.func isqrt32
 
isqrt32:
clr r0
clr r18
clr r19
clr r20
ldi r21, 1
clr r27
clr r30
clr r31
ldi r26, 16
1: lsl r22
rol r23
rol r24
rol r25
rol r0
rol r18
rol r19
rol r20
lsl r22
rol r23
rol r24
rol r25
rol r0
rol r18
rol r19
rol r20
brpl 2f
add r0, r21
adc r18, r27
adc r19, r30
adc r20, r31
rjmp 3f
2: sub r0, r21
sbc r18, r27
sbc r19, r30
sbc r20, r31
3: lsl r21
rol r27
rol r30
andi r21, 0b11111000
ori r21, 0b00000101
sbrc r20, 7
subi r21, 2
dec r26
brne 1b
lsr r30
ror r27
ror r21
lsr r30
ror r27
ror r21
mov r24, r21
mov r25, r27
ret
.endfunc
 
 
 
;-----------------------------------------------------------------------------:
; 16bit integer squareroot
;-----------------------------------------------------------------------------;
; uint8_t isqrt16 (
; uint16_t n
; );
;
; Return Value:
; Squareroot of n.
;
; Size = 33 words
; Clock = 181..189 cycles
; Stack = 0 byte
 
.global isqrt16
.func isqrt16
 
isqrt16:
clr r18
clr r19
ldi r20, 1
clr r21
ldi r22, 8
1: lsl r24
rol r25
rol r18
rol r19
lsl r24
rol r25
rol r18
rol r19
brpl 2f
add r18, r20
adc r19, r21
rjmp 3f
2: sub r18, r20
sbc r19, r21
3: lsl r20
rol r21
andi r20, 0b11111000
ori r20, 0b00000101
sbrc r19, 7
subi r20, 2
dec r22
brne 1b
lsr r21
ror r20
lsr r21
ror r20
mov r24, r20
ret
.endfunc
 
 
 
;-----------------------------------------------------------------------------:
; 16bit integer hypot (megaAVR is required)
;-----------------------------------------------------------------------------;
; uint16_t ihypot (
; int16_t x,
; int16_t y
; );
;
; Return Value:
; Squareroot of (x*x + y*y)
;
; Size = 42 words
; Clock = 581..597 cycles
; Stack = 0 byte
 
.global ihypot
.func ihypot
 
ihypot:
clr r26
sbrs r25, 7
rjmp 1f
com r24
com r25
adc r24, r26
adc r25, r26
1: sbrs r23, 7
rjmp 2f
com r22
com r23
adc r22, r26
adc r23, r26
2: mul r22, r22
movw r18, r0
mul r23, r23
movw r20, r0
mul r22, r23
add r19, r0
adc r20, r1
adc r21, r26
add r19, r0
adc r20, r1
adc r21, r26
mul r24, r24
movw r30, r0
mul r25, r25
add r18, r30
adc r19, r31
adc r20, r0
adc r21, r1
mul r24, r25
add r19, r0
adc r20, r1
adc r21, r26
add r19, r0
adc r20, r1
adc r21, r26
movw r24, r20
movw r22, r18
clr r1
rjmp isqrt32
.endfunc
 
 
 
/trunk/isqrt.h
0,0 → 1,11
#ifndef _ISQRT_H
#define _ISQRT_H
 
#include <inttypes.h>
 
// coded in assembler file
extern uint16_t isqrt32(uint32_t n);
extern uint8_t isqrt16(uint16_t n);
extern uint16_t ihypot(int16_t x, int16_t y);
 
#endif // _ISQRT_H
/trunk/makefile
116,7 → 116,7
# Even though the DOS/Win* filesystem matches both .s and .S the same,
# it will preserve the spelling of the filenames, and gcc itself does
# care about how the name is spelled on its command-line.
ASRC =
ASRC = isqrt.S