Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 18 → Rev 19

/branches/V0.1 killagreg/GPS.c
64,11 → 64,10
 
u8 OsdBar; // Direction home for OSD
s16 OsdDistance; // Distance home
s16 GPS_Nick;
s16 GPS_Roll;
s16 GPS_Yaw;
 
GPSParameter_t GPSParameter;
GPSStick_t GPSStick;
GPSParameter_t GPSParameter;
 
u8 logtext[100];
 
typedef enum
81,18 → 80,24
 
GpsFlightMode_t GpsFlightMode = GPS_MODE_FREE;
 
GPS_Pos_t GPSTargetPosition = {0,0,0, INVALID};
GPS_Pos_t GPSTHomePosition = {0,0,0, INVALID};
GPS_Pos_t PCGPSTargetPosition = {0,0,0, INVALID};
// the reference positions
GPS_Pos_t GPSTargetPosition = {0,0,0, INVALID};
GPS_Pos_t GPSTHomePosition = {0,0,0, INVALID};
GPS_Pos_t PCGPSTargetPosition = {0,0,0, INVALID};
 
 
void GPS_Neutral()
{
GPSStick.Nick = 0;
GPSStick.Roll = 0;
GPSStick.Yaw = 0;
}
 
//------------------------------------------------------------
// Init variables or send configuration to GPS module
void GPS_Init(void)
{
GPS_Nick = 0;
GPS_Roll = 0;
GPS_Yaw = 0;
GPS_Neutral();
 
OsdDistance = 0;
OsdBar = 0;
105,7 → 110,6
GPSParameter.Amplification = 100;
 
GpsFlightMode = GPS_MODE_FREE;
 
}
//-----------------------------------------------------------------------
void LogGPSData(void)
154,33 → 158,13
 
u8 Navigation(void)
{
static s8 GpsFix = 0, NewGpsMode = 0;
static GpsFlightMode_t oldGpsFlightMode = GPS_MODE_FREE;
static u32 beep_rythm;
static u32 GPSTimeout = 0;
static u32 GPSDataTimeout = 0;
 
 
 
switch(GPS_Data.Status)
{
case INVALID:
// no gps data available
break;
case NEWDATA:
// handle new gps data
GPSTimeout = SetDelay(2*GPS_Data.UpdateTime);
LogGPSData(); // write gps position to file
GPS_Data.Status = PROCESSED; // mark as processed
break;
case PROCESSED:
if(CheckDelay(GPSTimeout)) GPS_Data.Status = INVALID;
break;
}
 
 
 
beep_rythm++;
 
// update paramter
GPSParameter.ModeSwitch = Parameter.User1;
GPSParameter.Amplification = (float) Parameter.User2 / (100.0);
GPSParameter.P = (float) Parameter.User3;
187,9 → 171,9
GPSParameter.I = (float) Parameter.User4;
GPSParameter.D = (float) Parameter.User5;
GPSParameter.ACC = (float) Parameter.User6;
 
if(RC_Quality < 100) // RC-Signal lost
{
// in case of bad receiving conditions
if(FC.RC_Quality < 100)
{ // set fixed parameter
GPSParameter.ModeSwitch = 0;
GPSParameter.Amplification = 100;
GPSParameter.P = (float) 90;
199,43 → 183,68
}
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ GPS-Mode
//+ GPS-Mode Selection
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
oldGpsFlightMode = GpsFlightMode;
if(GPSParameter.ModeSwitch < 50) GpsFlightMode = GPS_MODE_AID;
else if(GPSParameter.ModeSwitch < 180) GpsFlightMode = GPS_MODE_FREE;
else GpsFlightMode = GPS_MODE_HOME;
// Mode changed?
if(GpsFlightMode != oldGpsFlightMode) BeepTime = 100; // indicate that mode has switched
// look for new data from GPS
switch(GPS_Data.Status)
{
case INVALID: // no gps data available
break;
case NEWDATA: // handle new gps data
// wait maximum of 3 times the normal data update time
GPSDataTimeout = SetDelay(3*GPS_Data.UpdateTime);
LogGPSData(); // write gps position to file
// set debug values
DebugOut.Analog[21] = (s16)GPS_Data.Speed_North;
DebugOut.Analog[22] = (s16)GPS_Data.Speed_East;
DebugOut.Analog[31] = GPS_Data.NumOfSats;
GPS_Data.Status = PROCESSED; // mark as processed
break;
case PROCESSED:
if(CheckDelay(GPSDataTimeout)) GPS_Data.Status = INVALID;
break;
}
 
if(GpsFlightMode != oldGpsFlightMode) // Mode changed
{
BeepTime = 100;
NewGpsMode = 1;
}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
 
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Fix okay
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((GPS_Data.Flags & FLAG_GPSFIXOK) && ((GPS_Data.NumOfSats >= GPS_SAT_MIN) || GpsFix))
{
GpsFix = 1; // hysteresis
// here is a good place to put your GPS code...
GPS_Nick = 0; // do nothing
GPS_Roll = 0; // do nothing
if((GPS_Data.Status != INVALID)) // there are valid data from the GPS
{
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Fix okay and enough sats
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if((GPS_Data.Flags & FLAG_GPSFIXOK) && (GPS_Data.NumOfSats >= GPS_SAT_MIN))
{
// here is a good place to put your GPS code...
GPSStick.Nick = 0; // do nothing
GPSStick.Roll = 0; // do nothing
}
else
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ No Fix or not enough sats
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
GPS_Neutral(); // disable gps control
if(!(GPS_Data.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
else if (GPS_Data.NumOfSats < GPS_SAT_MIN && !(beep_rythm % 5)) BeepTime = 10;
}
}
else
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ No Fix
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
{
GPS_Nick = 0; // reset value
GPS_Roll = 0; // reset value
/*
if(!(GPS_Data.Flags & FLAG_GPSFIXOK) && !(beep_rythm % 5)) BeepTime = 100;
else if (GPS_Data.NumOfSats < GPS_SAT_MIN && !(beep_rythm % 5)) BeepTime = 10;
*/
}
return (0);
else // GPS Data are invalid
{
GPS_Neutral(); // disable gps control
}
 
DebugOut.Analog[29] = GPSStick.Nick;
DebugOut.Analog[30] = GPSStick.Roll;
return (0);
}
 
/branches/V0.1 killagreg/GPS.h
34,10 → 34,17
extern u8 OsdBar;
extern s16 OsdDistance;
 
extern s16 GPS_Nick;
extern s16 GPS_Roll;
extern s16 GPS_Yaw;
typedef struct
{
s16 Nick;
s16 Roll;
s16 Yaw;
} __attribute__((packed)) GPSStick_t;
 
extern GPSStick_t GPSStick;
 
 
 
extern u8 Navigation(void);
extern void GPS_Init(void);
 
/branches/V0.1 killagreg/fat16.c
1686,7 → 1686,7
/****************************************************************************************************************************************/
/* Function: fexit_(const u8*); */
/* */
/* Description: This function cheacks if a file already exist. */
/* Description: This function checks if a file already exist. */
/* */
/* Returnvalue: 1 if the file exist else 0. */
/****************************************************************************************************************************************/
/branches/V0.1 killagreg/main.c
79,7 → 79,6
 
Param_t Parameter;
FC_t FC;
u8 RC_Quality = 0;
 
 
 
/branches/V0.1 killagreg/main.h
34,6 → 34,8
u8 Poti2;
u8 Poti3;
u8 Poti4;
u8 RC_Quality;
u8 MotorsOn;
} __attribute__((packed)) FC_t;
 
 
40,7 → 42,6
extern Param_t Parameter;
extern FC_t FC;
 
extern u8 RC_Quality;
 
 
 
/branches/V0.1 killagreg/menu.c
91,9 → 91,9
// Display with 20 characters in 4 lines
void LCD_PrintMenu(void)
{
static u8 MaxMenuItem = 7;
static u8 MaxMenuItem = 8;
static u8 MenuItem=0;
 
s16 i1,i2,i3;
// if KEY1 is activated goto previous menu item
if(RemoteButtons & KEY1)
{
154,47 → 154,80
LCD_printfxy(0,0,"Sats:%02d Fix:?? ", GPS_Data.NumOfSats);
break;
}
s16 i1,i2,i3;
i1 = (s16)(GPS_Data.Longitude/10000000L);
i2 = abs((s16)((GPS_Data.Longitude%10000000L)/10000L));
i3 = abs((s16)(((GPS_Data.Longitude%10000000L)%10000L)/10L));
LCD_printfxy(0,1,"Lon: %03d.%.3d%.3d deg",i1, i2, i3);
LCD_printfxy(0,1,"Lon: %3d.%.3d%.3d deg",i1, i2, i3);
i1 = (s16)(GPS_Data.Latitude/10000000L);
i2 = abs((s16)((GPS_Data.Latitude%10000000L)/10000L));
i3 = abs((s16)(((GPS_Data.Latitude%10000000L)%10000L)/10L));
LCD_printfxy(0,2,"Lat: %03d.%.3d%.3d deg",i1, i2, i3);
LCD_printfxy(0,2,"Lat: %3d.%.3d%.3d deg",i1, i2, i3);
i1 = (s16)(GPS_Data.Altitude/1000L);
i2 = abs((s16)(GPS_Data.Altitude%1000L));
LCD_printfxy(0,3,"Alt: %03d.%.3d m",i1, i2);
LCD_printfxy(0,3,"Alt: %3d.%.3d m",i1, i2);
}
break;
case 2: // RC stick controls from FC
case 2:
if (GPS_Data.Status == INVALID)
{
LCD_printfxy(0,0,"No GPS data! ");
LCD_printfxy(0,1," ");
LCD_printfxy(0,2," ");
LCD_printfxy(0,3," ");
}
else // newdata or processed
{
switch (GPS_Data.SatFix)
{
case SATFIX_NONE:
LCD_printfxy(0,0,"Sats:%02d Fix:None", GPS_Data.NumOfSats);
break;
case SATFIX_2D:
LCD_printfxy(0,0,"Sats:%02d Fix:2D ", GPS_Data.NumOfSats);
break;
case SATFIX_3D:
LCD_printfxy(0,0,"Sats:%02d Fix:3D ", GPS_Data.NumOfSats);
break;
default:
LCD_printfxy(0,0,"Sats:%02d Fix:?? ", GPS_Data.NumOfSats);
break;
}
i1 = (s16)(GPS_Data.Speed_North);
i2 = (s16)(GPS_Data.Speed_East);
i3 = (s16)(GPS_Data.Speed_Top);
LCD_printfxy(0,1,"Speed N: %4i cm/s",i1);
LCD_printfxy(0,2,"Speed E: %4i cm/s",i2);
LCD_printfxy(0,3,"Speed T: %4i cm/s",i3);
}
break;
case 3: // RC stick controls from FC
LCD_printfxy(0,0,"RC-Sticks" );
LCD_printfxy(0,1,"Ni:%4i Ro:%4i ",FC.StickNick, FC.StickRoll);
LCD_printfxy(0,2,"Gs:%4i Ya:%4i ",FC.StickGas, FC.StickYaw);
break;
case 3: // RC poti controls from FC
case 4: // RC poti controls from FC
LCD_printfxy(0,0,"RC-Potis" );
LCD_printfxy(0,1,"Po1:%3i Po2:%3i ",FC.Poti1, FC.Poti2);
LCD_printfxy(0,2,"Po3:%3i Po4:%3i ",FC.Poti3, FC.Poti4);
break;
case 4: // attitude from FC
case 5: // attitude from FC
LCD_printfxy(0,0,"IntNick: %5i", FromFlightCtrl.IntegralNick);
LCD_printfxy(0,1,"IntRoll: %5i", FromFlightCtrl.IntegralRoll);
LCD_printfxy(0,2,"AccNick: %5i", FromFlightCtrl.AccNick);
LCD_printfxy(0,3,"AccRoll: %5i", FromFlightCtrl.AccRoll);
break;
case 5: // gyros from FC
LCD_printfxy(0,0,"GyroNick: %4i", FromFlightCtrl.GyroNick);
case 6: // gyros from FC
LCD_printfxy(0,0,"GyroNick: %4i", FromFlightCtrl.GyroNick);
LCD_printfxy(0,1,"GyroRoll: %4i", FromFlightCtrl.GyroRoll);
LCD_printfxy(0,2,"GyroYaw: %4i", FromFlightCtrl.GyroYaw);
break;
case 6: // Remote Control Level from FC
LCD_printfxy(0,0,"RC-Level: %3i", RC_Quality);
LCD_printfxy(0,1,"CompHeading: %3i", I2C_Heading.Heading);
LCD_printfxy(0,2,"GyroHeading: %3i", FromFlightCtrl.GyroHeading);
case 7: // Remote Control Level from FC
LCD_printfxy(0,0,"RC-Level: %3i", FC.RC_Quality);
LCD_printfxy(0,1,"Motors On: %1i", FC.MotorsOn);
LCD_printfxy(0,2,"CompHeading: %3i", I2C_Heading.Heading);
LCD_printfxy(0,3,"GyroHeading: %3i", FromFlightCtrl.GyroHeading);
break;
case 7: // User Parameter
case 8: // User Parameter
LCD_printfxy(0,0,"UP1:%3i UP2:%3i ",Parameter.User1,Parameter.User2);
LCD_printfxy(0,1,"UP3:%3i UP4:%3i ",Parameter.User3,Parameter.User4);
LCD_printfxy(0,2,"UP5:%3i UP6:%3i ",Parameter.User5,Parameter.User6);
/branches/V0.1 killagreg/spi_slave.c
279,10 → 279,10
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt
 
ToFlightCtrl.CompassHeading = I2C_Heading.Heading;
ToFlightCtrl.GPS_Nick = GPS_Nick;
ToFlightCtrl.GPS_Roll = GPS_Roll;
ToFlightCtrl.GPS_Yaw = GPS_Yaw;
DebugOut.Analog[26] = I2C_Heading.Heading;
ToFlightCtrl.GPS_Nick = GPSStick.Nick;
ToFlightCtrl.GPS_Roll = GPSStick.Roll;
ToFlightCtrl.GPS_Yaw = GPSStick.Yaw;
DebugOut.Analog[26] = I2C_Heading.Heading;
// cycle spi commands
ToFlightCtrl.Command = SPI_CommandSequence[SPI_CommandCounter++];
if (SPI_CommandCounter >= sizeof(SPI_CommandSequence)) SPI_CommandCounter = 0;
340,7 → 340,8
FC.Poti2 = FromFlightCtrl.Param.Byte[5];
FC.Poti3 = FromFlightCtrl.Param.Byte[6];
FC.Poti4 = FromFlightCtrl.Param.Byte[7];
RC_Quality = FromFlightCtrl.Param.Byte[8];
FC.RC_Quality = FromFlightCtrl.Param.Byte[8];
FC.MotorsOn = FromFlightCtrl.Param.Byte[9];
break;
 
case SPI_CMD_CAL_COMPASS:
/branches/V0.1 killagreg/usb_desc.c
21,7 → 21,7
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Extern variables ----------------------------------------------------------*/
/* Extern variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/* USB Standard Device Descriptor */