Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1328 → Rev 1329

/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