Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2498 → Rev 2499

/trunk/fc.c
63,6 → 63,7
int MesswertNick,MesswertRoll,MesswertGier,RohMesswertNick,RohMesswertRoll;
int TrimNick, TrimRoll;
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
int BoatNeutralNick = 0,BoatNeutralRoll = 0,BoatNeutralGier = 0;
int Mittelwert_AccNick, Mittelwert_AccRoll;
unsigned int NeutralAccX=0, NeutralAccY=0;
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
361,17 → 362,18
NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
sucess = 0;
}
 
// restore from eeprom
BoatNeutralNick = (int16_t)GetParamWord(PID_GYRO_NICK);
BoatNeutralRoll = (int16_t)GetParamWord(PID_GYRO_ROLL);
BoatNeutralGier = (int16_t)GetParamWord(PID_GYRO_YAW);
 
if(FC_StatusFlags3 & FC_STATUS3_BOAT) // Read Gyro Data from eeprom
{
unsigned int nick, roll, gier;
// restore from eeprom
nick = (int16_t)GetParamWord(PID_GYRO_NICK);
roll = (int16_t)GetParamWord(PID_GYRO_ROLL);
gier = (int16_t)GetParamWord(PID_GYRO_YAW);
// strange settings?
if(((unsigned int) nick > (600 * 16)) || ((unsigned int) nick < (400 * 16))
|| ((unsigned int) roll > (600 * 16)) || ((unsigned int) roll < (400 * 16))
|| ((unsigned int) gier > (600 * 2)) || ((unsigned int) gier < (400 * 2)))
if(((unsigned int) BoatNeutralNick > (600 * 16)) || ((unsigned int) BoatNeutralNick < (400 * 16))
|| ((unsigned int) BoatNeutralRoll > (600 * 16)) || ((unsigned int) BoatNeutralRoll < (400 * 16))
|| ((unsigned int) BoatNeutralGier > (600 * 2)) || ((unsigned int) BoatNeutralGier < (400 * 2)))
{
printf("\n\rGyro calibration data not valid\r\n");
sucess = 0;
379,9 → 381,9
}
else
{
AdNeutralNick = nick;
AdNeutralRoll = roll;
AdNeutralGier = gier;
AdNeutralNick = BoatNeutralNick;
AdNeutralRoll = BoatNeutralRoll;
AdNeutralGier = BoatNeutralGier;
}
}
}
798,6 → 800,33
LIMIT_MIN_MAX(StickGasHover, 70, 175); // reserve some range for trim up and down
}
 
void ChannelAssingment(void)
{
cli();
ChannelNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
ChannelRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
ChannelYaw = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
ChannelGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((ExternalControl.Config & EC_VALID) && (Parameter_ExternalControl > 128))
{
ChannelNick += ExternalControl.Nick;
ChannelRoll += ExternalControl.Roll;
ChannelYaw += ExternalControl.Gier;
if(ExternalControl.Config & EC_GAS_ADD) ChannelGas += ExternalControl.Gas;
else
{
if(ExternalControl.Gas < ChannelGas) ChannelGas = ExternalControl.Gas; // the RC-Stick is the MAX value here
}
}
sei();
if(ChannelNick > 127) ChannelNick = 127; else if(ChannelNick < -127) ChannelNick = -127;
if(ChannelRoll > 127) ChannelRoll = 127; else if(ChannelRoll < -127) ChannelRoll = -127;
if(ChannelYaw > 127) ChannelYaw = 127; else if(ChannelYaw < -127) ChannelYaw = -127;
if(ChannelGas > 127) ChannelGas = 127; else if(ChannelGas < -127) ChannelGas = -127;
}
 
//############################################################################
//
831,7 → 860,7
if(GasIsZeroCnt == 30000) // in that case we have RC-Lost, but the MK is probably landed
{
StickGas = 0; // Hold Gas down in that case
ExternalControl.Gas = 0;
// ExternalControl.Gas = 0;
HooverGasEmergencyPercent = MIN_GAS;
}
GasMischanteil = StickGas;
1161,32 → 1190,9
static int stick_nick,stick_roll;
unsigned char stick_p;
ParameterZuordnung();
ChannelAssingment();
 
stick_p = EE_Parameter.Stick_P;
cli();
ChannelNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
ChannelRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
ChannelYaw = PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
ChannelGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Analoge Steuerung per Seriell
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((ExternalControl.Config & EC_VALID) && (Parameter_ExternalControl > 128))
{
ChannelNick += ExternalControl.Nick;
ChannelRoll += ExternalControl.Roll;
ChannelYaw += ExternalControl.Gier;
if(ExternalControl.Config & EC_GAS_ADD) ChannelGas += ExternalControl.Gas;
else
{
if(ExternalControl.Gas < ChannelGas) ChannelGas = ExternalControl.Gas; // the RC-Stick is the MAX value here
}
}
sei();
if(ChannelNick > 127) ChannelNick = 127; else if(ChannelNick < -127) ChannelNick = -127;
if(ChannelRoll > 127) ChannelRoll = 127; else if(ChannelRoll < -127) ChannelRoll = -127;
if(ChannelYaw > 127) ChannelYaw = 127; else if(ChannelYaw < -127) ChannelYaw = -127;
if(ChannelGas > 127) ChannelGas = 127; else if(ChannelGas < -127) ChannelGas = -127;
stick_nick = (stick_nick * 3 + ChannelNick * stick_p) / 4;
stick_roll = (stick_roll * 3 + ChannelRoll * stick_p) / 4;
cli();
1397,7 → 1403,7
DriftRoll += tmp_long2;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(ZaehlMessungen >= ABGLEICH_ANZAHL)
if(ZaehlMessungen >= ABGLEICH_ANZAHL) //alle 512ms
{
// static int cnt = 0;
// static char last_n_p,last_n_n,last_r_p,last_r_n;
1407,7 → 1413,6
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(EE_Parameter.Driftkomp && abs(Mittelwert_AccNick) < 200*4 && abs(Mittelwert_AccRoll) < 200*4 && !TrichterFlug && abs(MesswertGier) < 32/* && (FC_StatusFlags & FC_STATUS_FLY)*/)
{
DebugOut.Analog[16] = EE_Parameter.Driftkomp;
DriftNick -= DriftNick / (64 * (unsigned int) EE_Parameter.Driftkomp);
DriftRoll -= DriftRoll / (64 * (unsigned int) EE_Parameter.Driftkomp);
GierGyroFehler -= GierGyroFehler / (64 * (unsigned int) EE_Parameter.Driftkomp);
/trunk/fc.h
46,6 → 46,7
extern unsigned char FC_StatusFlags3;
extern void ParameterZuordnung(void);
extern unsigned char GetChannelValue(unsigned char ch); // gives the unsigned value of the channel
extern void ChannelAssingment(void);
 
#define Poti1 Poti[0]
#define Poti2 Poti[1]
96,6 → 97,7
extern unsigned char CareFree;
extern int MesswertNick,MesswertRoll,MesswertGier;
extern int AdNeutralNick,AdNeutralRoll,AdNeutralGier, Mittelwert_AccNick, Mittelwert_AccRoll;
extern int BoatNeutralNick,BoatNeutralRoll,BoatNeutralGier;
extern unsigned int NeutralAccX, NeutralAccY;
extern unsigned char HoehenReglerAktiv;
extern int NeutralAccZ;
156,5 → 158,6
extern unsigned char LowVoltageHomeActive;
extern unsigned char Parameter_MaximumAltitude;
extern char NeueKompassRichtungMerken;
 
#endif //_FC_H
 
/trunk/libfc1284.a
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
/trunk/libfc644.a
Cannot display: file marked as a binary type.
svn:mime-type = application/octet-stream
/trunk/main.c
72,6 → 72,7
void CalMk3Mag(void)
{
static unsigned char stick = 1;
ChannelAssingment();
if(ChannelNick > -20) stick = 0;
if((ChannelNick < -70) && !stick)
{
/trunk/spi.c
22,9 → 22,9
unsigned char SPITransferCompleted, SPI_ChkSum;
unsigned char SPI_RxDataValid,NaviDataOkay = 250;
 
unsigned char SPI_CommandSequence[] = { SPI_FCCMD_STICK, SPI_FCCMD_USER, SPI_FCCMD_SLOW,
SPI_FCCMD_STICK, SPI_FCCMD_VERSION, SPI_FCCMD_BL_ACCU,
SPI_FCCMD_STICK, SPI_FCCMD_USER, SPI_FCCMD_MISC,SPI_FCCMD_SERVOS,
unsigned char SPI_CommandSequence[] = { SPI_FCCMD_STICK, SPI_FCCMD_USER, SPI_FCCMD_SLOW,SPI_FCCMD_BL_ACCU,
SPI_FCCMD_STICK, SPI_FCCMD_MISC, SPI_FCCMD_BL_ACCU,
SPI_FCCMD_STICK, SPI_FCCMD_USER, SPI_FCCMD_BL_ACCU,
SPI_FCCMD_STICK, SPI_FCCMD_PARAMETER2, SPI_FCCMD_BL_ACCU
};
unsigned char SPI_CommandCounter = 0;
150,7 → 150,7
void UpdateSPI_Buffer(void)
{
// signed int tmp;
static unsigned char motorindex, oldcommand = SPI_NCCMD_VERSION, slow_command;
static unsigned char motorindex, oldcommand = SPI_NCCMD_VERSION, slow_command = 0;
ToNaviCtrl.IntegralNick = (int) (IntegralNick / (long)(EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad
ToNaviCtrl.IntegralRoll = (int) (IntegralRoll / (long)(EE_Parameter.GyroAccFaktor * 4)); // etwa in 0.1 Grad
ToNaviCtrl.GyroCompass = (10 * ErsatzKompass) / GIER_GRAD_FAKTOR;
199,7 → 199,22
switch(slow_command)
{
case 0:
ToNaviCtrl.Command = SPI_FCCMD_PARAMETER1; slow_command = 1;
ToNaviCtrl.Command = SPI_FCCMD_VERSION;
ToNaviCtrl.Param.Byte[0] = VERSION_MAJOR;
ToNaviCtrl.Param.Byte[1] = VERSION_MINOR;
ToNaviCtrl.Param.Byte[2] = VERSION_PATCH;
ToNaviCtrl.Param.Byte[3] = NC_SPI_COMPATIBLE;
ToNaviCtrl.Param.Byte[4] = PlatinenVersion;
ToNaviCtrl.Param.Byte[5] = EE_Parameter.LandingSpeed;
ToNaviCtrl.Param.Byte[6] = EE_Parameter.ComingHomeAltitude;
ToNaviCtrl.Param.Byte[7] = EE_Parameter.AutoPhotoAtitudes;
ToNaviCtrl.Param.Byte[8] = VersionInfo.BL_Firmware;
ToNaviCtrl.Param.Byte[9] = 0;
ToNaviCtrl.Param.Int[5] = FlugMinutenGesamt; // 10 & 11
slow_command++;
break;
case 1:
ToNaviCtrl.Command = SPI_FCCMD_PARAMETER1;
ToNaviCtrl.Param.Byte[0] = (unsigned char) BattLowVoltageWarning; //0.1V
ToNaviCtrl.Param.Byte[1] = EE_Parameter.NaviGpsGain; // Parameters for the Naviboard
ToNaviCtrl.Param.Byte[2] = EE_Parameter.NaviGpsP;
212,18 → 227,49
ToNaviCtrl.Param.Byte[9] = EE_Parameter.NaviWindCorrection;
ToNaviCtrl.Param.Byte[10] = EE_Parameter.NaviAccCompensation;
ToNaviCtrl.Param.Byte[11] = EE_Parameter.NaviAngleLimitation;
slow_command++;
break;
case 2:
ToNaviCtrl.Command = SPI_FCCMD_SLOW2;
ToNaviCtrl.Param.Int[0] = BoatNeutralNick; // 0 & 1
ToNaviCtrl.Param.Int[1] = BoatNeutralRoll; // 2 & 3
ToNaviCtrl.Param.Int[2] = BoatNeutralGier; // 4 & 5
ToNaviCtrl.Param.Byte[6] = EE_Parameter.CamOrientation;
ToNaviCtrl.Param.Byte[7] = EE_Parameter.CompassOffset;
ToNaviCtrl.Param.Byte[8] = Parameter_GlobalConfig;
ToNaviCtrl.Param.Byte[9] = Parameter_ExtraConfig;
ToNaviCtrl.Param.Byte[10] = EE_Parameter.OrientationAngle;
ToNaviCtrl.Param.Byte[11] = EE_Parameter.GlobalConfig3;
slow_command++;
break;
case 3:
ToNaviCtrl.Command = SPI_FCCMD_SLOW3;
ToNaviCtrl.Param.Byte[0] = Parameter_ServoNickControl;
ToNaviCtrl.Param.Byte[1] = Parameter_ServoRollControl;
ToNaviCtrl.Param.Byte[2] = EE_Parameter.NaviDescendRange; // in 10m
ToNaviCtrl.Param.Byte[3] = Parameter_MaximumAltitude;
ToNaviCtrl.Param.Byte[4] = 0;
ToNaviCtrl.Param.Byte[5] = 0;
ToNaviCtrl.Param.Byte[6] = 0;
ToNaviCtrl.Param.Byte[7] = 0;
ToNaviCtrl.Param.Byte[8] = 0;
ToNaviCtrl.Param.Byte[9] = 0;
ToNaviCtrl.Param.Byte[10] = 0;
ToNaviCtrl.Param.Byte[11] = 0;
slow_command++;
break;
default:
ToNaviCtrl.Command = SPI_FCCMD_NEUTRAL; slow_command = 0;
ToNaviCtrl.Command = SPI_FCCMD_NEUTRAL;
ToNaviCtrl.Param.Int[0] = AdNeutralNick; // 0 & 1
ToNaviCtrl.Param.Int[1] = AdNeutralRoll; // 2 & 3
ToNaviCtrl.Param.Int[2] = AdNeutralGier; // 4 & 5
ToNaviCtrl.Param.Byte[6] = EE_Parameter.Driftkomp;
ToNaviCtrl.Param.Byte[7] = HoverGas / 4;
ToNaviCtrl.Param.Byte[8] = 0;
ToNaviCtrl.Param.Byte[9] = 0;
ToNaviCtrl.Param.Byte[10] = 0;
ToNaviCtrl.Param.Byte[11] = 0;
ToNaviCtrl.Param.Byte[7] = EE_Parameter.NaviPH_LoginTime;
ToNaviCtrl.Param.Byte[8] = EE_Parameter.Receiver;
ToNaviCtrl.Param.Byte[9] = EE_Parameter.NaviGpsPLimit;
ToNaviCtrl.Param.Byte[10] = EE_Parameter.NaviGpsILimit;
ToNaviCtrl.Param.Byte[11] = EE_Parameter.NaviGpsDLimit;
slow_command = 0;
break;
}
break;
240,8 → 286,7
#else
ToNaviCtrl.Param.Byte[1] = 0;
#endif
ToNaviCtrl.Param.Byte[2] = EE_Parameter.LandingSpeed;
ToNaviCtrl.Param.Byte[3] = EE_Parameter.ComingHomeAltitude;
ToNaviCtrl.Param.Int[1] = Capacity.UsedCapacity; // mAh // 2 & 3
ToNaviCtrl.Param.Byte[4] = LowVoltageHomeActive;
ToNaviCtrl.Param.Byte[5] = ToNC_Load_WP_List;
ToNaviCtrl.Param.Byte[6] = ToNC_Load_SingePoint;
251,7 → 296,7
ToNC_Store_SingePoint = 0;
if(Parameter_KompassWirkung) ToNaviCtrl.Param.sInt[4] = KompassSollWert; // Pos. 8 & 9
else ToNaviCtrl.Param.sInt[4] = ErsatzKompassInGrad; // answer with the compass value if the Compass effect is zero
ToNaviCtrl.Param.Byte[10] = EE_Parameter.AutoPhotoAtitudes;
ToNaviCtrl.Param.Byte[10] = FC_StatusFlags3;
ToNaviCtrl.Param.Byte[11] = EE_Parameter.SingleWpSpeed;
break;
case SPI_FCCMD_STICK:
287,44 → 332,18
ToNaviCtrl.Param.Byte[0] = 5;
}
else ToNaviCtrl.Param.Byte[0] = WinkelOut.CalcState;
ToNaviCtrl.Param.Byte[1] = EE_Parameter.NaviPH_LoginTime;
ToNaviCtrl.Param.Int[1] = (int)(HoehenWert/5);
ToNaviCtrl.Param.Int[2] = (int)(SollHoehe/5);
ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviGpsPLimit;
ToNaviCtrl.Param.Byte[7] = EE_Parameter.NaviGpsILimit;
ToNaviCtrl.Param.Byte[8] = EE_Parameter.NaviGpsDLimit;
ToNaviCtrl.Param.Byte[1] = HoverGas / 4;
ToNaviCtrl.Param.Int[1] = (int)(HoehenWert/5); //2 & 3
ToNaviCtrl.Param.Int[2] = (int)(SollHoehe/5); //4 & 5
ToNaviCtrl.Param.Byte[6] = VersionInfo.HardwareError[0];
ToNaviCtrl.Param.Byte[7] = VersionInfo.HardwareError[1];
VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // VersionInfo.HardwareError[0] = 0;
VersionInfo.HardwareError[1] &= FC_ERROR1_MIXER;
ToNaviCtrl.Param.Byte[8] = DebugOut.Analog[28]; // I2C-Error counter
ToNaviCtrl.Param.Byte[9] = (unsigned char) SenderOkay;
ToNaviCtrl.Param.Byte[10] = NC_Wait_for_LED;
ToNaviCtrl.Param.Byte[11] = DebugOut.Analog[7] / 4; //GasMischanteil
break;
case SPI_FCCMD_VERSION:
ToNaviCtrl.Param.Byte[0] = VERSION_MAJOR;
ToNaviCtrl.Param.Byte[1] = VERSION_MINOR;
ToNaviCtrl.Param.Byte[2] = VERSION_PATCH;
ToNaviCtrl.Param.Byte[3] = NC_SPI_COMPATIBLE;
ToNaviCtrl.Param.Byte[4] = PlatinenVersion;
ToNaviCtrl.Param.Byte[5] = VersionInfo.HardwareError[0];
ToNaviCtrl.Param.Byte[6] = VersionInfo.HardwareError[1];
VersionInfo.HardwareError[0] &= ~FC_ERROR0_CAREFREE; // VersionInfo.HardwareError[0] = 0;
VersionInfo.HardwareError[1] &= FC_ERROR1_MIXER;
ToNaviCtrl.Param.Byte[7] = EE_Parameter.CompassOffset;
ToNaviCtrl.Param.Byte[8] = Parameter_GlobalConfig;
ToNaviCtrl.Param.Byte[9] = Parameter_ExtraConfig;
ToNaviCtrl.Param.Byte[10] = EE_Parameter.OrientationAngle;
ToNaviCtrl.Param.Byte[11] = EE_Parameter.GlobalConfig3;
break;
case SPI_FCCMD_SERVOS:
ToNaviCtrl.Param.Int[0] = Capacity.UsedCapacity; // mAh
ToNaviCtrl.Param.Byte[2] = Parameter_ServoNickControl;
ToNaviCtrl.Param.Byte[3] = Parameter_ServoRollControl;
ToNaviCtrl.Param.Byte[4] = DebugOut.Analog[28]; // I2C-Error counter
ToNaviCtrl.Param.Byte[5] = FC_StatusFlags3;
ToNaviCtrl.Param.Byte[6] = EE_Parameter.NaviDescendRange; // in 10m
ToNaviCtrl.Param.Byte[7] = Parameter_MaximumAltitude;
ToNaviCtrl.Param.Int[4] = FlugMinutenGesamt; // 8 & 9
ToNaviCtrl.Param.Byte[10] = EE_Parameter.CamOrientation;
ToNaviCtrl.Param.Byte[11] = VersionInfo.BL_Firmware;
break;
}
if(SPI_RxDataValid)
/trunk/spi.h
61,11 → 61,12
#define SPI_FCCMD_MISC 12
#define SPI_FCCMD_PARAMETER1 13
#define SPI_FCCMD_VERSION 14
#define SPI_FCCMD_SERVOS 15
#define SPI_FCCMD_SLOW3 15
#define SPI_FCCMD_BL_ACCU 16
#define SPI_FCCMD_PARAMETER2 17
#define SPI_FCCMD_NEUTRAL 18
#define SPI_FCCMD_SLOW 19
#define SPI_FCCMD_SLOW2 20
 
struct str_ToNaviCtrl
{
/trunk/version.txt
752,6 → 752,12
- NC-Yawing rate limited to 100°/sec
- ExpandBaro faster -> 80ms instead of 700ms
- Altitude measurement re-calibrated (the measured value was about 5% too high)
 
2.09c (06.01.2015)
- transmit receiver type to NC
- transmit stored neutral values (BOAT) to NC
- SPI communication optimized -> some Data are faster, some are slower
- BL-State (Current, Temperature,...) are now faster transferred for Logging
toDo:
- CalAthmospheare nachführen