/trunk/main.c |
---|
94,6 → 94,7 |
s16 GeoMagDec = 0; // local magnetic declination in 0.1 deg |
u8 ErrorGpsFixLost = 0; |
u8 FromFC_LoadWP_List = 0; |
u8 ToFC_MaxWpListIndex = 3; |
u8 ClearFCStatusFlags = 0; |
u8 StopNavigation = 0; |
volatile u32 PollingTimeout = 10000; |
534,7 → 535,6 |
// initialize i2c busses (needs Timer 1) |
I2CBus_Init(I2C0); |
I2CBus_Init(I2C1); |
// initialize fat16 partition on sd card (needs Timer 1) |
Fat16_Init(); |
// initialize NC params |
586,7 → 586,7 |
Settings_GetParamValue(PID_SEND_NMEA, &NMEA_Interval); |
UART1_PutString("\r\n"); |
CompassValueErrorCount = 0; |
I2CBus(Compass_I2CPort)->Timeout = SetDelay(3000); |
for (;;) // the endless main loop |
{ |
PollingTimeout = 5; |
594,21 → 594,28 |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
if(FromFC_LoadWP_List) |
{ |
WPL_Store.Index = FromFC_LoadWP_List; |
WPL_Store.Index = (FromFC_LoadWP_List & ~0x80); |
if(WPL_Store.Index <= ToFC_MaxWpListIndex) |
{ |
if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
{ |
if(FromFC_LoadWP_List & 0x80)// -> load relative |
{ |
if(!PointList_Move(1, &(GPSData.Position))) // try to move wp-list so that 1st entry matches the current position |
{ |
PointList_Clear(); |
} |
} |
if(FromFC_LoadWP_List & 0x80)// -> load relative |
{ |
if(NCFlags & NC_FLAG_FREE || NaviData.TargetPositionDeviation.Distance > 7*10) |
{ // take actual position |
if(!PointList_Move(1,&(GPSData.Position),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
} |
else |
{ // take last target position |
if(!PointList_Move(1, &(NaviData.TargetPosition),NaviData.CompassHeading)) PointList_Clear(); // try to move wp-list so that 1st entry matches the current position |
} |
} |
if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
BeepTime = 150; |
} |
FromFC_LoadWP_List = 0; |
} |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
617,12 → 624,14 |
{ |
SD_WatchDog = 30000; |
if(SDCardInfo.Valid == 1) Logging_Update(); // could be block some time for at max. 2 seconds, therefore move time critical part of the mainloop into the ISR of timer 1 |
else if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
else |
{ |
ToFC_MaxWpListIndex = 0; |
if(FC.StatusFlags & FC_STATUS_START) SD_LoggingError = 100; |
} |
if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
} |
/* |
if(CheckDelay(ftimer)) |
{ |
/trunk/main.h |
---|
14,7 → 14,7 |
#define VERSION_MAJOR 2 |
#define VERSION_MINOR 3 |
#define VERSION_PATCH 5 |
#define VERSION_PATCH 6 |
// 0 = A |
// 1 = B |
// 2 = C |
206,7 → 206,7 |
extern u8 ErrorCode; |
extern u8 StopNavigation; |
extern u8 ErrorGpsFixLost; |
extern u8 FromFC_LoadWP_List; |
extern u8 FromFC_LoadWP_List, ToFC_MaxWpListIndex; |
extern volatile u32 SPIWatchDog; // stop Navigation if this goes to zero |
extern volatile u32 SD_WatchDog; // stop Logging if this goes to zero |
extern volatile u32 PollingTimeout; |
/trunk/menu.c |
---|
490,20 → 490,11 |
static u8 index = 1, ret = 0; |
if(Keys & KEY3) // next step |
{ |
if(index < 4) index++; |
if(index < ToFC_MaxWpListIndex) index++; |
else index = 1; |
} |
if(Keys & KEY4) |
{ |
WPL_Store.Index = index; |
if(PointList_ReadFromFile(&WPL_Store) == WPL_OK) |
{ |
if(FC.StatusFlags & FC_STATUS_FLY) PointList_WPActive(TRUE); |
GPS_pWaypoint = PointList_WPBegin(); // updates POI index |
BeepTime = 150; |
} |
} |
LCD_printfxy(0,0,"Load WPL" ); |
if(Keys & KEY4) FromFC_LoadWP_List = index; |
LCD_printfxy(0,0,"Load WPL (fix)" ); |
LCD_printfxy(0,1,"Name: %s", WPL_Store.Name); |
// 12345678901234567890 |
LCD_printfxy(0,2,"Points Index "); |
/trunk/spi_slave.c |
---|
397,7 → 397,7 |
ToFlightCtrl.Param.Byte[5] = NaviData.WaypointNumber; // number of stored waypoints |
ToFlightCtrl.Param.Int[3] = NaviData.TargetPositionDeviation.Distance / 10; |
ToFlightCtrl.Param.Byte[8] = NaviData.TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached |
ToFlightCtrl.Param.Byte[9] = 0; |
ToFlightCtrl.Param.Byte[9] = ToFC_MaxWpListIndex; |
ToFlightCtrl.Param.Byte[10] = 0; |
ToFlightCtrl.Param.Byte[11] = 0; |
SpeakHoTT = 0; |
/trunk/ubx.c |
---|
323,6 → 323,23 |
UbxVelNed.Status = PROCESSED; // ready for new data |
} // EOF all itow are equal |
} // EOF all ubx messages received |
//++++++++++++++++++++++++++++++++ |
// Please do not delete |
// This helps me for testing |
//++++++++++++++++++++++++++++++++ |
//GPSData.Position.Longitude = 1517409123L; // Hamilton, Australia |
//GPSData.Position.Latitude = -329294773L; // Hamilton, Australia |
//++++++++ |
//GPSData.Position.Longitude =-1556010020L; // Alaska |
//GPSData.Position.Latitude = 629581270L; // Alaska |
//++++++++ |
//GPSData.Position.Longitude =-584343419L; // Buenos aires |
//GPSData.Position.Latitude = -345464421L; // Buenos aires |
//++++++++ |
//GPSData.Position.Longitude =1683362691L; // Neuseeland |
//GPSData.Position.Latitude = -465945926L; // Neuseeland |
} |
/trunk/waypoints.c |
---|
709,10 → 709,10 |
} |
// move actual point list to ref pos., the point in the list marked by index gets the RefPos afterwards |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos) |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle) |
{ |
u8 retval = 0; |
GPS_Pos_t RefPos_old; |
GPS_Pos_t FirstPoint; |
GPS_Pos_Deviation_t RefDeviation; |
// check inputs for plausibility; |
721,7 → 721,7 |
if(pRefPos->Status == INVALID) return(retval); |
// try to copy the old reference in point list to a local buffer |
if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &RefPos_old)) |
if(GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint)) |
{ |
u8 i; |
// for each point position in the list |
728,8 → 728,10 |
for(i = 0; i < PointCount; i++) |
{ |
retval = 0; |
// Save altitude of that point |
pRefPos->Altitude = PointList[i].Position.Altitude; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
if(!GPSPos_Deviation(&(PointList[i].Position), &RefPos_old, &RefDeviation)) break; |
if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
// copy of the new reference position into this list place |
if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
// move new reference according to the deviation of the old reference |
736,6 → 738,29 |
retval = GPSPos_ShiftCartesian(&(PointList[i].Position), RefDeviation.North, RefDeviation.East); |
if(!retval) break; |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
// Now rotate around the reference point |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
if(RotationAngle > 0) |
{ |
GPSPos_Copy(&(PointList[RefIndex-1].Position), &FirstPoint); // Rotate around the reference point |
for(i = 0; i < PointCount; i++) |
{ |
retval = 0; |
// Save altitude of that point |
pRefPos->Altitude = PointList[i].Position.Altitude; |
// calculate deviation form old ref, i.e the north and east shift of each point in the list from the reference position |
if(!GPSPos_Deviation(&(PointList[i].Position), &FirstPoint, &RefDeviation)) break; |
// copy of the new reference position into this list place |
if(!GPSPos_Copy(pRefPos, &(PointList[i].Position))) break; |
// move new reference according to the deviation of the old reference |
retval = GPSPos_ShiftGeodetic(&(PointList[i].Position), (RefDeviation.Bearing + 180 + RotationAngle)%360,RefDeviation.Distance ); |
// Now rotate the heading positions if they are fixed angles |
if(PointList[i].Heading >= 0 && PointList[i].Heading <= 360) PointList[i].Heading = (PointList[i].Heading + RotationAngle) % 360; |
if(!retval) break; |
} |
} |
// ++++++++++++++++++++++++++++++++++++++++++++++ |
} // else ref pos old not copied! |
if(!retval) PointList_Clear(); |
return(retval); |
/trunk/waypoints.h |
---|
69,7 → 69,7 |
// load actual point list from SD card |
u8 PointList_ReadFromFile(WPL_Store_t * pWPL_Store); |
// move actual point list to ref pos., the point in the list marked by RefIndex get the RefPos afterwards |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos); |
u8 PointList_Move(u8 RefIndex, GPS_Pos_t* pRefPos, u16 RotationAngle); |
#endif // _WAYPOINTS_H |