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