/beta/Code Redesign killagreg/fc.c |
---|
66,6 → 66,7 |
#include "twimaster.h" |
#include "timer2.h" |
#include "mymath.h" |
#include "isqrt.h" |
#ifdef USE_KILLAGREG |
#include "mm3.h" |
#include "gps.h" |
647,8 → 648,8 |
/************************************************************************/ |
void MotorControl(void) |
{ |
int16_t tmp_int; |
int32_t tmp_long, tmp_long2, tmp_long3; |
int16_t tmp_int, tmp_int2; |
int32_t tmp_long, tmp_long2; |
// Mixer Fractions that are combined for Motor Control |
int16_t YawMixFraction, GasMixFraction, NickMixFraction, RollMixFraction; |
1443,6 → 1444,7 |
DebugOut.Analog[10] = RC_Quality; |
DebugOut.Analog[11] = YawGyroHeading / GYRO_DEG_FACTOR; |
DebugOut.Analog[19] = CompassCalState; |
DebugOut.Analog[20] = ServoNickValue; |
DebugOut.Analog[29] = NCSerialDataOkay; |
DebugOut.Analog[30] = GPSStickNick; |
DebugOut.Analog[31] = GPSStickRoll; |
1484,9 → 1486,6 |
PDPartYaw = (int32_t)(GyroYaw * 2 * (int32_t)GyroYawPFactor) / (256L / STICK_GAIN) + (int32_t)(IntegralGyroYaw * GyroYawIFactor) / (2 * (44000 / STICK_GAIN)); |
//DebugOut.Analog[21] = PDPartNick; |
//DebugOut.Analog[22] = PDPartRoll; |
// limit control feedback |
#define SENSOR_LIMIT (4096 * 4) |
LIMIT_MIN_MAX(PDPartNick, -SENSOR_LIMIT, SENSOR_LIMIT); |
1608,11 → 1607,10 |
// calculate cos of nick and roll angle used for projection of the vertical hoover gas |
tmp_long = IntegralGyroNick/GYRO_DEG_FACTOR; // nick angle in deg |
tmp_long *= tmp_long; |
tmp_long2 = IntegralGyroRoll/GYRO_DEG_FACTOR; // roll angle in deg |
tmp_long2 *= tmp_long2; |
CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
tmp_int = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); // nick angle in deg |
tmp_int2 = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); // roll angle in deg |
CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); |
DebugOut.Analog[22] = CosAttitude; |
LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1736,10 → 1734,10 |
HCGas -= tmp_int; |
// strech control output by inverse attitude projection 1/cos |
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; |
1777,29 → 1775,29 |
if(HooverGasFilter == 0) HooverGasFilter = HOOVER_GAS_AVERAGE * (uint32_t)(GasMixFraction); // init estimation |
if(abs(ReadingVario) < 100) // only on small vertical speed |
{ |
tmp_long3 = (int32_t)GasMixFraction; // take current thrust |
tmp_long3 *= CosAttitude; // apply attitude projection |
tmp_long3 /= 8192; |
tmp_long2 = (int32_t)GasMixFraction; // take current thrust |
tmp_long2 *= CosAttitude; // apply attitude projection |
tmp_long2 /= 8192; |
// average vertical projected thrust |
if(ModelIsFlying < 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(ModelIsFlying < 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(ModelIsFlying < 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(ParamSet.Height_HooverBand) |
/beta/Code Redesign killagreg/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 |
/beta/Code Redesign killagreg/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 |
/beta/Code Redesign killagreg/main.c |
---|
333,9 → 333,9 |
{ |
UpdateMotor = 0; // reset Flag, is enabled every 2 ms by ISR of timer0 |
//J4HIGH; |
J4HIGH; |
MotorControl(); |
//J4LOW; |
J4LOW; |
SendMotorData(); // the flight control code |
RED_OFF; |
/beta/Code Redesign killagreg/makefile |
---|
5,7 → 5,7 |
#------------------------------------------------------------------- |
VERSION_MAJOR = 0 |
VERSION_MINOR = 75 |
VERSION_PATCH = 13 |
VERSION_PATCH = 14 |
VERSION_SERIAL_MAJOR = 10 # Serial Protocol Major Version |
VERSION_SERIAL_MINOR = 1 # Serial Protocol Minor Version |
142,7 → 142,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 |
/beta/Code Redesign killagreg/mymath.c |
---|
77,28 → 77,3 |
else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant |
} |
// Integer square root |
// For details of the algorithm see the article http://www.embedded.com/98/9802fe2.htm |
uint32_t c_sqrt(uint32_t a) |
{ |
uint32_t rem = 0; |
uint32_t root = 0; |
uint8_t i; |
for(i = 0; i < 16; i++) |
{ |
root <<= 1; |
rem = ((rem << 2) + (a >> 30)); |
a <<= 2; |
root++; |
if(root <= rem) |
{ |
rem -= root; |
root++; |
} |
else root--; |
} |
return (root >> 1); |
} |
/beta/Code Redesign killagreg/mymath.h |
---|
6,6 → 6,5 |
extern int16_t c_sin_8192(int16_t angle); |
extern int16_t c_cos_8192(int16_t angle); |
extern int16_t c_atan2(int16_t y, int16_t x); |
extern uint32_t c_sqrt(uint32_t a); |
#endif // _MYMATH_H |
/beta/Code Redesign killagreg/rc.c |
---|
232,8 → 232,8 |
RED_ON; |
} |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
if(index == 5) J3HIGH; else J3LOW; |
if(index == 6) J4HIGH; else J4LOW; |
//if(index == 5) J3HIGH; else J3LOW; |
//if(index == 6) J4HIGH; else J4LOW; |
if(CPUType != ATMEGA644P) // not used as TXD1 |
{ |
if(index == 7) J5HIGH; else J5LOW; |
298,8 → 298,8 |
} |
index++; // next channel |
// demux sum signal for channels 5 to 7 to J3, J4, J5 |
if(index == 5) J3HIGH; else J3LOW; |
if(index == 6) J4HIGH; else J4LOW; |
//if(index == 5) J3HIGH; else J3LOW; |
//if(index == 6) J4HIGH; else J4LOW; |
if(CPUType != ATMEGA644P) // not used as TXD1 |
{ |
if(index == 7) J5HIGH; else J5LOW; |
/beta/Code Redesign killagreg/timer2.c |
---|
197,7 → 197,6 |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
// range servo pulse width |
if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
261,7 → 260,6 |
RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
ServoNickValue /= MULTIPLYER; |
DebugOut.Analog[20] = ServoNickValue; |
break; |
288,7 → 286,6 |
} |
RemainingPulse += ServoRollValue - (256 / 2) * MULTIPLYER; // shift ServoRollValue to center position |
ServoRollValue /= MULTIPLYER; |
DebugOut.Analog[21] = ServoRollValue; |
break; |
default: // other servo channels |
/beta/Code Redesign killagreg/version.txt |
---|
279,7 → 279,7 |
- Bugfix Messbereichsumschaltung des Luftdrucksensors springt |
- Auflösung des Luftdrucks nun bis auf 1 cm (5mal feiner) zur genaueren Berechnung des D-Anteils |
- Unterstützung von Warnings-Bitmasks für die J16, J17-Outputs bei Unterspannung |
- Unterspannung für einezelne Zelle´n von 3.2V auf 3.3V angehoben (9.6V --> 9.9V für 3S) |
- Unterspannung für einzelne Zelle´n von 3.2V auf 3.3V angehoben (9.6V --> 9.9V für 3S) |
0.75d H.Buss 13.8.2009 |
- RC-Routine: Empfangsausfall soll sicherer erkannt werden |
327,9 → 327,13 |
- Gyro_Gier_P und Gyro_Gier_I eingeführt |
- I2C_fehler-Zähler jetzt in den Analogdaten |
Anpassungen bzgl. V0.75n |
G.Stobrawa 30.9.2009: |
0.75o H.Buss 01.10.2009 |
- der Höhenregler wird jetzt nur alle 10ms bearbeitet |
- Baro-Messbereichserweiterung auch bei Poti als Sollwert |
Anpassungen bzgl. V0.75o |
G.Stobrawa 1.10.2009: |
- Code stärker modularisiert und restrukturiert |
- viele Kommentare zur Erklärug eingefügt |
- konsequent englische Variablennamen |