/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 |