Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 586 → Rev 587

/trunk/gpx.c
127,6 → 127,7
sprintf(string, "<extensions>\r\n"); fputs_(string, doc->file);
sprintf(string, "<MK_Time>%imin</MK_Time>\r\n",FlugMinutenGesamt);fputs_(string, doc->file);
sprintf(string, "<GpsVersion>%d.%03d</GpsVersion>\r\n",GPS_Version/1000,GPS_Version%1000);fputs_(string, doc->file);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
sprintf(string, "<License>\r\n"); fputs_(string, doc->file);
if(tmp_license->User[0] != 0)
{
140,16 → 141,52
sprintf(string, "<info>No License installed</info>\r\n"); fputs_(string, doc->file);
}
sprintf(string, "</License>\r\n"); fputs_(string, doc->file);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
sprintf(string, "<settings>\r\n"); fputs_(string, doc->file);
sprintf(string, "<Number>%i</Number>\r\n",Parameter.ActiveSetting);fputs_(string, doc->file);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Reciever Type
sprintf(string, "<Receiver>");fputs_(string, doc->file);
switch(Parameter.ReceiverType)
{
case RECEIVER_PPM: sprintf(string, "PPM"); break;
case RECEIVER_SPEKTRUM: sprintf(string, "SPEKTRUM"); break;
case RECEIVER_SPEKTRUM_HI_RES: sprintf(string, "SPEKTRUM_HI_RES"); break;
case RECEIVER_SPEKTRUM_LOW_RES: sprintf(string, "SPEKTRUM_LOW_RES"); break;
case RECEIVER_JETI: sprintf(string, "JETI"); break;
case RECEIVER_ACT_DSL: sprintf(string, "ACT_DSL"); break;
case RECEIVER_HOTT: sprintf(string, "HOTT"); break;
case RECEIVER_SBUS: sprintf(string, "SBUS"); break;
case RECEIVER_USER: sprintf(string, "USER"); break;
default: sprintf(string, "? %d",Parameter.ReceiverType); break;
}
fputs_(string, doc->file);
sprintf(string, "</Receiver>\r\n"); fputs_(string, doc->file);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
sprintf(string, "<CompassOffset>%i</CompassOffset>\r\n",FC.FromFC_CompassOffset/10);fputs_(string, doc->file);
sprintf(string, "<FCOrientation>%i</FCOrientation>\r\n",Parameter.OrientationAngle*15);fputs_(string, doc->file);
sprintf(string, "<GeoMag>%i.%1d</GeoMag>\r\n",GeoMagDec/10,GeoMagDec%10);fputs_(string, doc->file);
sprintf(string, "<GeoMag>%i.%1d</GeoMag>\r\n",GeoMagDec/10,abs(GeoMagDec)%10);fputs_(string, doc->file);
sprintf(string, "<Neutral>%d,%d,%d</Neutral>\r\n", FC.AdNeutralNick,FC.AdNeutralRoll,FC.AdNeutralYaw);fputs_(string, doc->file);
sprintf(string, "<Kalibr.>%d,%d,%d</Kalibr.>\r\n", FC.BoatNeutralNick,FC.BoatNeutralRoll,FC.BoatNeutralYaw);fputs_(string, doc->file);
if((FC.StatusFlags3 & FC_STATUS3_BOAT)) {sprintf(string, "<BoatMode>On</BoatMode>\r\n");fputs_(string, doc->file);}
if(Parameter.Driftkomp) {sprintf(string, "<DriftComp>%i</DriftComp>\r\n",Parameter.Driftkomp);fputs_(string, doc->file);};
if(Compass_I2CPort == NCMAG_PORT_EXTERN) sprintf(string, "<MagSensor>external (%i)</MagSensor>\r\n",NCMAG_Orientation);
else sprintf(string, "<MagSensor>internal</MagSensor>\r\n");fputs_(string, doc->file);
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Magnetometer
if(Compass_I2CPort == NCMAG_PORT_EXTERN) sprintf(string, "<MagSensor>external,(%i),",NCMAG_Orientation);
else sprintf(string, "<MagSensor>internal,");
fputs_(string, doc->file);
switch(NCMAG_SensorType)
{
case TYPE_HMC5843: sprintf(string, "HMC"); break;
case TYPE_LSM303DLH: sprintf(string, "DLH"); break;
case TYPE_LSM303DLM: sprintf(string, "DLM"); break;
default: sprintf(string, "? %d",NCMAG_SensorType); break;
}
fputs_(string, doc->file);
sprintf(string, "</MagSensor>\r\n");fputs_(string, doc->file);
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
if(AbsoluteFlyingAltitude) {sprintf(string, "<MaxAltitude>%i</MaxAltitude>\r\n",AbsoluteFlyingAltitude);fputs_(string, doc->file);}
if(AbsoluteFlyingRange_m) {sprintf(string, "<FlyRange>%i</FlyRange>\r\n",AbsoluteFlyingRange_m);fputs_(string, doc->file);}
if(AutoDescendRange_m) {sprintf(string, "<Descend>%i</Descend>\r\n",AutoDescendRange_m);fputs_(string, doc->file);};
162,6 → 199,7
if((Parameter.GlobalConfig3 & CFG3_VARIO_FAILSAFE)) {sprintf(string, "<VarioFailsafe>On</VarioFailsafe>\r\n");fputs_(string, doc->file);}
if((Parameter.ExtraConfig & CFG_3_3V_REFERENCE)) {sprintf(string, "<3.3V_Reference>On</3.3V_Reference>\r\n");fputs_(string, doc->file);}
if((Parameter.ExtraConfig & CFG_IGNORE_MAG_ERR_AT_STARTUP)) {sprintf(string, "<IgnoreMagnetError>ON!</IgnoreMagnetError>\r\n");fputs_(string, doc->file);}
if(FC.FromFC_DisableDeclination) {sprintf(string, "<DisableDeclinationCalc>ON!</DisableDeclinationCalc>\r\n");fputs_(string, doc->file);}
 
sprintf(string, "<Bytes>%02x,%02x,%02x</Bytes>\r\n",Parameter.GlobalConfig,Parameter.ExtraConfig,Parameter.GlobalConfig3);fputs_(string, doc->file);
sprintf(string, "</settings>\r\n"); fputs_(string, doc->file);
439,8 → 477,8
break;
case 4:
// Compass in deg
if(SimulationFlags) sprintf(string, "<Compass>%03d,%03d,%03d,%03d</Compass>\r\n",SimulatedDirection,SimulatedDirection,CompassSetpointCorrected/10,SimulatedDirection);
else sprintf(string, "<Compass>%03d,%03d,%03d,%03d</Compass>\r\n",FromFlightCtrl.GyroHeading/10,ToFlightCtrl.CompassHeading,CompassSetpointCorrected/10,GyroCompassCorrected/10);
if(SimulationFlags) sprintf(string, "<Compass>%03d,%03d,%03d.%1d,%03d.0</Compass>\r\n",SimulatedDirection,SimulatedDirection,CompassSetpointCorrected/10,CompassSetpointCorrected%10,SimulatedDirection);
else sprintf(string, "<Compass>%03d,%03d,%03d.%1d,%03d.%1d</Compass>\r\n",FromFlightCtrl.GyroHeading/10,ToFlightCtrl.CompassHeading,CompassSetpointCorrected/10,CompassSetpointCorrected%10,GyroCompassCorrected/10,GyroCompassCorrected%10);
fputs_(string, doc->file);
// magnetic field
sprintf(string, "<MagnetField>%03d</MagnetField>\r\n",(u16) (EarthMagneticFieldFiltered/5));
/trunk/main.h
36,7 → 36,7
// 18 = S
 
#ifndef FOLLOW_ME
#define FC_SPI_COMPATIBLE 71 // <------------------
#define FC_SPI_COMPATIBLE 72 // <------------------
#else
#define FC_SPI_COMPATIBLE 0xFF
#endif
121,6 → 121,19
#define CFG_TEACHABLE_CAREFREE 0x40
#define CFG_IGNORE_MAG_ERR_AT_STARTUP 0x80
 
// Parameter.ReceiverType defines for the receiver selection
#define RECEIVER_PPM 0
#define RECEIVER_SPEKTRUM 1
#define RECEIVER_SPEKTRUM_HI_RES 2
#define RECEIVER_SPEKTRUM_LOW_RES 3
#define RECEIVER_JETI 4
#define RECEIVER_ACT_DSL 5
#define RECEIVER_HOTT 6
#define RECEIVER_SBUS 7
#define RECEIVER_USER 8
 
#define RECEIVER_UNKNOWN 0xFF
 
#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
187,6 → 200,7
u8 DescendRange; // in 10m
u8 MaximumAltitude; // in m
u8 Driftkomp;
u8 ReceiverType;
} __attribute__((packed)) Param_t;
 
typedef struct
213,6 → 227,9
u16 AdNeutralNick;
u16 AdNeutralRoll;
u16 AdNeutralYaw;
u16 BoatNeutralNick;
u16 BoatNeutralRoll;
u16 BoatNeutralYaw;
} __attribute__((packed)) FC_t; // from FC
 
extern Param_t Parameter;
/trunk/spi_slave.c
107,7 → 107,8
u8 SPI_CommandSequence[] = { SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN, SPI_NCCMD_VERSION };
u8 SPI_CommandCounter = 0;
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
s32 HeadFreeStartAngle = 0;
s32 HeadFreeStartAngle = 0; // in 0,1°
s32 CompassDirectionAtMotorStart = 0; // in 0,1°
s16 FC_WP_EventChannel = 0, LogFC_WP_EventChannel = 0, FC_WP_EventChannel_Processed = 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
u32 ToFC_AltitudeRate = 0;
s32 ToFC_AltitudeSetpoint = 0;
600,7 → 601,6
Parameter.User6 = FromFlightCtrl.Param.Byte[5];
Parameter.User7 = FromFlightCtrl.Param.Byte[6];
Parameter.User8 = FromFlightCtrl.Param.Byte[7];
 
FC.RealStatusFlags = FromFlightCtrl.Param.Byte[8];
if(FC.RealStatusFlags & FC_STATUS_MOTOR_RUN) SimulationFlags = 0; // stop the simulation if the motors would really start
 
616,6 → 616,7
if(FC.StatusFlags & FC_STATUS_CALIBRATE && !FCCalibActive)
{
HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
CompassDirectionAtMotorStart = HeadFreeStartAngle;
Compass_Init();
FCCalibActive = 10;
FC_is_Calibrated = 0;
635,6 → 636,7
{
if(Compass_Heading != -1) HeadFreeStartAngle = (3600 + Compass_Heading * 10 + GeoMagDec) % 3600;
else HeadFreeStartAngle = GyroCompassCorrected;
CompassDirectionAtMotorStart = HeadFreeStartAngle;
}
 
if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
711,8 → 713,7
case SPI_FCCMD_PARAMETER2:
CHK_POTI_MM(Parameter.AutoPhotoDistance,FromFlightCtrl.Param.Byte[0],0,255);
if(FromFlightCtrl.Param.Byte[1]) FC.FromFC_SpeakHoTT = FromFlightCtrl.Param.Byte[1]; // will be cleared in the SD-Logging
Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[2];
Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[3];
FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[1]; // 2 & 3
Parameter.FromFC_LowVoltageHomeActive = FromFlightCtrl.Param.Byte[4];
if(FromFlightCtrl.Param.Byte[5]) FromFC_LoadWP_List = FromFlightCtrl.Param.Byte[5];
if(FromFlightCtrl.Param.Byte[6]) FromFC_Load_SinglePoint = FromFlightCtrl.Param.Byte[6];
719,7 → 720,7
if(FromFlightCtrl.Param.Byte[7]) FromFC_Save_SinglePoint = FromFlightCtrl.Param.Byte[7];
CompassSetpoint = FromFlightCtrl.Param.sInt[4] * 10; // 8 & 9
CompassSetpointCorrected = (3600 + CompassSetpoint + FC.FromFC_CompassOffset + GeoMagDec) % 3600;
CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[10],0,255);
FC.StatusFlags3 = FromFlightCtrl.Param.Byte[10];
Parameter.SingleWpSpeed = FromFlightCtrl.Param.Byte[11];
break;
case SPI_FCCMD_STICK:
748,13 → 749,17
}
// else CompassCalState = 0;
}
Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[1];
HoverGas = FromFlightCtrl.Param.Byte[1];
NaviData.Variometer = (NaviData.Variometer + 2 * (FromFlightCtrl.Param.sInt[1] - NaviData.Altimeter)) / 2; // provisorisch
FC.Altimeter = FromFlightCtrl.Param.sInt[1]; // in 5cm
if(!(SimulationFlags & SIMULATION_ACTIVE)) NaviData.SetpointAltitude = FromFlightCtrl.Param.sInt[2]; // in 5cm
CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[6],0,255);
CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[7],0,255);
CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[8],0,255);
FC.Error[0] |= FromFlightCtrl.Param.Byte[6];
FC.Error[1] |= FromFlightCtrl.Param.Byte[7];
DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
if(FC.Error[0] || FC.Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.StatusRed |= AMPEL_FC;
else DebugOut.StatusRed &= ~AMPEL_FC;
FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[8];
FC.RC_Quality = FromFlightCtrl.Param.Byte[9];
NaviData.RC_Quality = FC.RC_Quality;
NC_Wait_for_LED = FromFlightCtrl.Param.Byte[10];
764,27 → 769,35
LoggingGasCnt++;
break;
 
case SPI_FCCMD_SERVOS:
FC.BAT_UsedCapacity = FromFlightCtrl.Param.Int[0];
ServoParams.NickControl = FromFlightCtrl.Param.Byte[2];
ServoParams.RollControl = FromFlightCtrl.Param.Byte[3];
FC_I2C_ErrorConter = FromFlightCtrl.Param.Byte[4];
FC.StatusFlags3 = FromFlightCtrl.Param.Byte[5];
Parameter.DescendRange = FromFlightCtrl.Param.Byte[6];
Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[7];
FlugMinutenGesamt = FromFlightCtrl.Param.Int[4]; // 8 & 9
Parameter.CamOrientation = FromFlightCtrl.Param.Byte[10];
UART_VersionInfo.BL_Firmware = FromFlightCtrl.Param.Byte[11];
break;
 
case SPI_FCCMD_VERSION:
case SPI_FCCMD_VERSION: // slow!
FC_Version.Major = FromFlightCtrl.Param.Byte[0];
FC_Version.Minor = FromFlightCtrl.Param.Byte[1];
FC_Version.Patch = FromFlightCtrl.Param.Byte[2];
FC_Version.Compatible = FromFlightCtrl.Param.Byte[3];
FC_Version.Hardware = FromFlightCtrl.Param.Byte[4];
FC.Error[0] |= FromFlightCtrl.Param.Byte[5];
FC.Error[1] |= FromFlightCtrl.Param.Byte[6];
Parameter.FromFC_LandingSpeed = FromFlightCtrl.Param.Byte[5];
Parameter.ComingHomeAltitude = FromFlightCtrl.Param.Byte[6];
CHK_POTI_MM(Parameter.AutoPhotoAltitudes,FromFlightCtrl.Param.Byte[7],0,255);
UART_VersionInfo.BL_Firmware = FromFlightCtrl.Param.Byte[8];
// 9
FlugMinutenGesamt = FromFlightCtrl.Param.Int[5]; // 10 & 11
break;
case SPI_FCCMD_NEUTRAL: // slow!
FC.AdNeutralNick = FromFlightCtrl.Param.Int[0];
FC.AdNeutralRoll = FromFlightCtrl.Param.Int[1];
FC.AdNeutralYaw = FromFlightCtrl.Param.Int[2];
Parameter.Driftkomp = FromFlightCtrl.Param.Byte[6];
Parameter.NaviPH_LoginTime = FromFlightCtrl.Param.Byte[7];
Parameter.ReceiverType = FromFlightCtrl.Param.Byte[8];
CHK_POTI_MM(Parameter.NaviGpsPLimit,FromFlightCtrl.Param.Byte[9],0,255);
CHK_POTI_MM(Parameter.NaviGpsILimit,FromFlightCtrl.Param.Byte[10],0,255);
CHK_POTI_MM(Parameter.NaviGpsDLimit,FromFlightCtrl.Param.Byte[11],0,255);
break;
case SPI_FCCMD_SLOW2: // slow!
FC.BoatNeutralNick = FromFlightCtrl.Param.Int[0]; // 0 & 1
FC.BoatNeutralRoll = FromFlightCtrl.Param.Int[1]; // 2 & 3
FC.BoatNeutralYaw = FromFlightCtrl.Param.Int[2]; // 4 & 5
Parameter.CamOrientation = FromFlightCtrl.Param.Byte[6];
if(FromFlightCtrl.Param.Byte[7] >= 68 && FromFlightCtrl.Param.Byte[7] <= 188)
{
FC.FromFC_DisableDeclination = 1;
800,18 → 813,22
Parameter.ExtraConfig = FromFlightCtrl.Param.Byte[9];
Parameter.OrientationAngle = FromFlightCtrl.Param.Byte[10];
Parameter.GlobalConfig3 = FromFlightCtrl.Param.Byte[11];
DebugOut.StatusGreen |= AMPEL_FC; // status of FC Present
DebugOut.StatusGreen |= AMPEL_BL; // status of BL Present
if(FC.Error[0] || FC.Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.StatusRed |= AMPEL_FC;
else DebugOut.StatusRed &= ~AMPEL_FC;
break;
case SPI_FCCMD_NEUTRAL:
FC.AdNeutralNick = FromFlightCtrl.Param.Int[0];
FC.AdNeutralRoll = FromFlightCtrl.Param.Int[1];
FC.AdNeutralYaw = FromFlightCtrl.Param.Int[2];
Parameter.Driftkomp = FromFlightCtrl.Param.Byte[6];
HoverGas = FromFlightCtrl.Param.Byte[7];
case SPI_FCCMD_SLOW3: // slow!
ServoParams.NickControl = FromFlightCtrl.Param.Byte[0];
ServoParams.RollControl = FromFlightCtrl.Param.Byte[1];
Parameter.DescendRange = FromFlightCtrl.Param.Byte[2];
Parameter.MaximumAltitude = FromFlightCtrl.Param.Byte[3];
// 4
// 5
// 6
// 7
// 8
// 9
// 10
// 11
break;
 
default:
break;
}
876,7 → 893,7
// if we got it
if (FC_Version.Major != 0xFF)
{
sprintf(msg, " FC V%d.%02d%c HW:%d.%02d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
sprintf(msg, " FC V%d.%02d%c HW:%d.%d", FC_Version.Major, FC_Version.Minor, 'a'+FC_Version.Patch, FC_Version.Hardware/10,FC_Version.Hardware%10);
UART1_PutString(msg);
}
else UART1_PutString("\n\r not found!");
/trunk/spi_slave.h
50,11 → 50,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 // kommt nicht vor
#define SPI_FCCMD_SLOW2 20
 
#define CHK_POTI(b,a) { if(a < 248) b = a; else b = FC.Poti[255 - a]; }
#define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max); }
69,7 → 70,7
extern s32 Kalman_MaxFusion;
extern s32 ToFcGpsZ;
extern s32 ToFC_Rotate_C, ToFC_Rotate_S;
extern s32 HeadFreeStartAngle;
extern s32 HeadFreeStartAngle,CompassDirectionAtMotorStart;
extern s16 FC_WP_EventChannel,LogFC_WP_EventChannel,FC_WP_EventChannel_Processed;
extern u32 ToFC_AltitudeRate;
extern s32 ToFC_AltitudeSetpoint;