/trunk/compass.c |
---|
195,14 → 195,15 |
if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing |
if(check_value_counter == 2000) |
if(check_value_counter == 2000) // 2 seconds no change of the compass values |
{ |
UART1_PutString("\n\r Init Mag.-Sensor"); |
Compass_Init(); // 2 seconds no change of the compass value |
Compass_Init(); |
} |
else if(check_value_counter > 5000) |
if(check_value_counter > 5000) // 5 seconds no change of the compass values |
{ |
Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault |
Compass_Heading = -1; // -> probably a compass-fault |
} |
else check_value_counter++; |
/trunk/gpspos.h |
---|
34,6 → 34,8 |
u8 GPSPos_Copy(GPS_Pos_t* pGPSPosSrc, GPS_Pos_t* pGPSPosTgt); |
// calculate the deviation from the source position to the target position |
u8 GPSPos_Deviation(GPS_Pos_t* pReferencePos, GPS_Pos_t* pTargetPos, GPS_Pos_Deviation_t* pDeviation); |
// clear position deviation |
u8 GPSPos_Deviation_Clear(GPS_Pos_Deviation_t* pDeviation); |
// Move the gps position according to north and east shift in cm |
u8 GPSPos_ShiftCartesian(GPS_Pos_t* pGPSPos, s32 NorthShift, s32 EastShift); |
// Move the gps position according to the direction of bearing in deg by the given distance in cm |
/trunk/main.c |
---|
481,8 → 481,6 |
{ |
IENABLE; |
VIC_ITCmd(EXTIT3_ITLine,DISABLE); // disable irq |
//Compass_Update(); // update compass communication |
//Analog_Update(); // get new ADC values |
if(PollingTimeout == 0) |
{ |
601,6 → 599,8 |
if(!SD_WatchDog) UART1_PutString("\n\rSD-Watchdog - Logging aborted\n\r"); |
} |
/* |
if(CheckDelay(ftimer)) |
{ |
/trunk/mymath.c |
---|
10,7 → 10,7 |
s16 c_sin_8192(s16 angle) |
{ |
s8 m,n; |
s8 m, n; |
s16 sinus; |
// avoid negative angles |
123,3 → 123,31 |
else if( x < 0) q = 4*((1<<15)-(1<<13)) - q; |
return(q); |
} |
// retuns the largest integer whose square is less than or equal to the |
u32 isqrt(u32 value) |
{ |
u32 rem = 0, root =0, idx; |
for(idx = 0; idx < 16; idx++) |
{ |
root <<= 1; |
rem = ((rem << 2) + (value >> 30)); |
value <<= 2; |
root++; |
if(root <= rem) |
{ |
rem -= root; |
root++; |
} |
else |
{ |
root--; |
} |
} |
return(root >> 1); |
} |
/trunk/mymath.h |
---|
13,6 → 13,9 |
s32 c_atan2_546(s32 y, s32 x); |
// fast arccos implementation |
s16 c_arccos2(s32 a,s32 b); |
s16 c_arccos2(s32 a, s32 b); |
// integer based square root |
u32 isqrt(u32 value); |
#endif // _MYMATH_H |
/trunk/ncmag.c |
---|
903,10 → 903,9 |
break; |
case TYPE_LSM303DLH: |
case TYPE_LSM303DLM: |
delay = 20; // next cycle after 20 ms |
if(s-- || (Compass_I2CPort == NCMAG_PORT_INTERN)) |
{ |
delay = 20; // next cycle after 20 ms |
NCMAG_GetMagVector(5); |
} |
else // having an external compass, read every 50th cycle the ACC vec |
1061,11 → 1060,11 |
if(NCMAG_Present) // do only short init ! , full init was called before |
{ |
// reset I2C Bus |
I2CBus_Deinit(Compass_I2CPort); |
I2CBus_Init(Compass_I2CPort); |
//I2CBus_Deinit(Compass_I2CPort); |
//I2CBus_Init(Compass_I2CPort); |
// try to reconfigure senor |
NCMAG_ConfigureSensor(); |
NCMAG_Update(1); |
//NCMAG_Update(1); |
} |
else // full init |
{ |
/trunk/params.c |
---|
35,6 → 35,21 |
} |
break; |
case NCPARAMS_WP_LIST_LOAD: |
// read wp list from file |
if(PointList_ReadFromFile(*pvalue)) |
{ //and move all wp relative so that 1st wp gets current positition |
// PointList_Move(1, &GPSData.Position); |
} |
else PointList_Clear(); |
break; |
break; |
case NCPARAMS_WP_LIST_SAVE: |
PointList_SaveToFile(*pvalue); |
break; |
default: |
break; |
} |
/trunk/params.h |
---|
10,6 → 10,8 |
#define NCPARAMS_SHOW_TARGET 5 |
#define NCPARAMS_WP_EVENT_ONCE 6 |
#define NCPARAMS_WP_EVENT_FOREVER 7 |
#define NCPARAMS_WP_LIST_LOAD 8 |
#define NCPARAMS_WP_LIST_SAVE 9 |
#define NCPARAM_STATE_UNDEFINED 0 |
#define NCRARAM_STATE_VALID 1 |
/trunk/uart1.c |
---|
529,15 → 529,6 |
} |
break; |
case 2: |
// read wp list from file |
if(PointList_ReadFromFile(SerialMsg.pData[2])) |
{ //and move all wp relative so that 1st wp gets current positition |
PointList_Move(1, &GPSData.Position); |
} |
else PointList_Clear(); |
break; |
default: |
break; |
} |
/trunk/ubx.c |
---|
299,21 → 299,21 |
GPSData.Flags = (GPSData.Flags & 0xf0) | (UbxSol.Flags & 0x0f); // we take only the lower bits |
GPSData.NumOfSats = UbxSol.numSV; |
GPSData.SatFix = UbxSol.GPSfix; |
GPSData.Position_Accuracy = UbxSol.PAcc; |
GPSData.Speed_Accuracy = UbxSol.SAcc; |
GPSData.Position_Accuracy = UbxSol.PAcc; // in steps of 1cm |
GPSData.Speed_Accuracy = UbxSol.SAcc; // in steps of 1cm/s |
SetGPSTime(&SystemTime); // update system time |
// NAV POSLLH |
GPSData.Position.Status = INVALID; |
GPSData.Position.Longitude = UbxPosLlh.LON; |
GPSData.Position.Latitude = UbxPosLlh.LAT; |
GPSData.Position.Altitude = UbxPosLlh.HMSL; |
GPSData.Position.Longitude = UbxPosLlh.LON; // in steps of 1E-7 deg |
GPSData.Position.Latitude = UbxPosLlh.LAT; // in steps of 1E-7 deg |
GPSData.Position.Altitude = UbxPosLlh.HMSL; // in steps of 1 mm |
GPSData.Position.Status = NEWDATA; |
// NAV VELNED |
GPSData.Speed_East = UbxVelNed.VEL_E; |
GPSData.Speed_North = UbxVelNed.VEL_N; |
GPSData.Speed_Top = -UbxVelNed.VEL_D; |
GPSData.Speed_Ground = UbxVelNed.GSpeed; |
GPSData.Heading = UbxVelNed.Heading; |
GPSData.Speed_East = UbxVelNed.VEL_E; // in steps of 1cm/s |
GPSData.Speed_North = UbxVelNed.VEL_N; // in steps of 1cm/s |
GPSData.Speed_Top = -UbxVelNed.VEL_D; // in steps of 1cm/s |
GPSData.Speed_Ground = UbxVelNed.GSpeed; // in steps of 1cm/s |
GPSData.Heading = UbxVelNed.Heading; //in steps of 1E-5 deg |
GPSData.Status = NEWDATA; // new data available |
} // EOF if(GPSData.Status != NEWDATA) |
/trunk/waypoints.c |
---|
463,7 → 463,7 |
retval = 1; |
} |
} // EOF if(Fat16_IsValid()) |
UART1_PutString("no file system found!\r\n"); |
else UART1_PutString("no file system found!\r\n"); |
return(retval); |
} |