Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 502 → Rev 503

/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