/**
* Creates the ini file for the odom and initializes the odom values to initial values
*/
void Odometry_Boot(Sensor * sensor,SensorVariable * start)
{
/* MAKE THESE GLOBAL*/
LOG.ODOMETRY("BOOTING THE ODOMETERY SENSOR");
if(fileExists("config/odometry.ini"))
{
dictionary* config = iniparser_load("config/odometry.ini");
Odometry_leftWheelMeterPerTick = iniparser_getdouble(config,"WheelEncoderConstant:MeterPerTickLeft",0);
Odometry_rightWheelMeterPerTick = iniparser_getdouble(config,"WheelEncoderConstant:MeterPerTickRight",0);
Odometry_trackInMeters = iniparser_getdouble(config,"DistanceBetweenWheels:track",0);
Odometry_cvarL = iniparser_getdouble(config,"Variance:LeftWheel",.000049871);
Odometry_cvarR = iniparser_getdouble(config,"Variance:RightWheel",.000073805);
iniparser_freedict(config);
LOG.ODOMETRY("Odometry Ini file loaded");
}
else
{
LOG.ERR("odomentry.ini was not found");
}
matrix Variance = Matrix_Create(2,2);
Variance = Matrix_SetValue(Variance,1,1,.01);
Variance = Matrix_SetValue(Variance,2,2,.01);
sensor->state->Variance=Variance;
sensor->state->State= Matrix_Create(3,1);
sensor->H = Matrix_Create(2,6);
sensor->H = Matrix_SetValue(sensor->H,1,4,PSOUpdateRateInS);
sensor->H = Matrix_SetValue(sensor->H,1,5,Odometry_trackInMeters*PSOUpdateRateInS/2.0);
sensor->H = Matrix_SetValue(sensor->H,2,4,PSOUpdateRateInS);
sensor->H = Matrix_SetValue(sensor->H,2,5,-Odometry_trackInMeters*PSOUpdateRateInS/2.0);
Odometry_rightWheelTicks=Microprocessor->getRightWheelTicks();
Odometry_leftWheelTicks=Microprocessor->getLeftWheelTicks();
}
/**
* Updates the odometry values
*/
void Odometry_Update(Sensor * sensor,const SensorVariable state)
{
LOG.ODOMETRY("Starting Odomentry Sensor Update");
/*Set to true*/
sensor->hasBeenUpdated = 1;
/*Get New data and figure out the Dticks in each wheel*/
int32_t newLeftWheelTicks = Microprocessor->getLeftWheelTicks();
int32_t newRightWheelTicks = Microprocessor->getRightWheelTicks();
LOG.ODOMETRY("LEFT WHEEL TICKS = %d",newLeftWheelTicks);
LOG.ODOMETRY("RIGHT WHEEL TICKS = %d",newRightWheelTicks);
int32_t dticksLeftWheel = newLeftWheelTicks - Odometry_leftWheelTicks;
int32_t dticksRightWheel = newRightWheelTicks - Odometry_rightWheelTicks;
Odometry_leftWheelTicks = newLeftWheelTicks;
Odometry_rightWheelTicks = newRightWheelTicks;
float dmLeftWheel = (float)dticksLeftWheel * Odometry_leftWheelMeterPerTick;
float dmRightWheel = (float)dticksRightWheel * Odometry_rightWheelMeterPerTick;
float YleftEnc = dmLeftWheel - 1.0/2.0 * PSOUpdateRateInS *(2.0 * Matrix_GetVel(state.State)+Odometry_trackInMeters*Matrix_GetOmega(state.State));
float YrightEnc = dmRightWheel - 1.0/2.0 * PSOUpdateRateInS *(2.0 * Matrix_GetVel(state.State)-Odometry_trackInMeters*Matrix_GetOmega(state.State));
float VarLeftEnc = dmLeftWheel*dmLeftWheel * Odometry_cvarL + Odometry_small_const_variance;
float VarRightEnc = dmRightWheel*dmRightWheel * Odometry_cvarR + Odometry_small_const_variance;
/*Set the variances.
These should be the addition of the previous variance and the variance created by this sensor*/
/* If you want to make the variances larger, it should be done in the config file, not as this multiply */
matrix Variance = Matrix_Create(2,2);
Variance = Matrix_SetValue(Variance,1,1,VarLeftEnc);
Variance = Matrix_SetValue(Variance,2,2,VarRightEnc);
sensor->state->Variance = Variance;
/* Set the new location based on the changes in the odometry sensor*/
matrix Residual = Matrix_Create(2,1);
Residual = Matrix_SetValue(Residual,1,1,YleftEnc);
Residual = Matrix_SetValue(Residual,2,1,YrightEnc);
sensor->state->State=Residual;
}
Matrix * Matrix_CreateDuplicate(const Matrix *m)
{
Matrix * ret;
uint r;
ret = Matrix_Create(m->dimension);
assert(ret);
for(r=0;r<m->dimension;r++)
{
memcpy(ret->rows[r]->element,m->rows[r]->element,sizeof(double)*m->dimension);
}
return ret;
}
/**
* Initliazes the Compass with the .ini file
*/
void Compass_Boot(Sensor * sensor,SensorVariable * start)
{
LOG.COMPASS("Compassing Booting");
if(fileExists("config/compass.ini"))
{
dictionary* config = iniparser_load("config/compass.ini");
COMPASS_Variance = iniparser_getdouble(config,"Variance:Variance",0.523598776);
COMPASS_Declination= iniparser_getdouble(config,"Declination:Declination",-7.5);
iniparser_freedict(config);
LOG.COMPASS("compass.ini file loaded");
}
else
{
LOG.COMPASS("compass.ini was not found");
}
sensor->H = Matrix_Create(1,5);
sensor->H = Matrix_SetValue(sensor->H,1,3,1);
sensor->state->State= Matrix_Create(1,1);
sensor->state->Variance = Matrix_Create(1,1);
LOG.COMPASS("Compassing Booted");
}
Matrix * Matrix_CreateFromFile(FILE *fp)
{
Matrix * m;
uint dim;
assert(fp);
fscanf(fp,"%d",&dim);
if ( dim == 0 )
return NULL;
m = Matrix_Create(dim);
Matrix_SetFromFile(m,fp);
return m;
}
void Matrix_SolveLinearEquation(const Matrix *m,const Vector *b,Vector *a)
{
Matrix * inv;
assert( a && b && m );
assert( a->length == b->length && a->length == m->dimension );
assert( a != b );
// solve m a = b for a
inv = Matrix_Create(m->dimension);
Matrix_ComputeInverse(m,inv);
Matrix_Apply(inv,b,a); // a = m^(-1) b
Matrix_Destroy(inv);
}
// Reads the GPS and stores the values
void _ReadPosPacket(GPS_Packet_Header * header, Sensor * sensor,const SensorVariable state)
{
GPS_Packet_BestPos * bestPosPt = (GPS_Packet_BestPos*)&header->dataStart;
GPS_Packet_BestPos bestPos;
memcpy(&bestPos,bestPosPt,sizeof(GPS_Packet_BestPos));
bestPos.longitude = _Convert64Double(bestPos.longitude);
bestPos.latitude = _Convert64Double(bestPos.latitude);
bestPos.latitudeStdDev = _Convert32Float(bestPos.latitudeStdDev);
bestPos.longitudeStdDev = _Convert32Float(bestPos.longitudeStdDev);
bestPos.solStat = _Convert32Int(bestPos.solStat);
bestPos.posType = _Convert32Int(bestPos.posType);
bestPos.differentialAge = _Convert32Float(bestPos.differentialAge);
bestPos.solutionAge = _Convert32Float(bestPos.solutionAge);
LOG.GPS("LAT %Lf LONG %Lf",bestPos.latitude,bestPos.longitude);
LOG.GPS("Satellites Used = %d ",bestPos.satellitesUsed);
LOG.GPS("Satellites Tracked = %d ",bestPos.satellitesTracked);
GPS_MakeGPSPacket(&bestPos, &gpsToBroadcast);
gps_is_new = TRUE;
if (GPS_networkOutputOnly)
{
sensor->hasBeenUpdated = FALSE;
}
else
{
// Check for this 0 case due to funky ness
if(bestPos.solStat == sol_status_computed)
{
LOG.GPS("Solution status computed");
if(bestPos.posType == pos_type_single || bestPos.posType == pos_type_omnistar || bestPos.posType == pos_type_omnistar_hp)
{
LOG.GPS("Position type was single, omnistar or omnistarHP: %u", bestPos.posType);
if(bestPos.satellitesUsed<12 && bestPos.satellitesUsed > 0)
{
LOG.GPS("Make satellites used PASS");
if(bestPos.latitude<GPS_latHighLimit && bestPos.latitude > GPS_latLowLimit)
{
LOG.GPS("LAtitude LIMIT PASS");
if(bestPos.longitude < GPS_longHighLimit && bestPos.longitude > GPS_longLowLimit)
{
LOG.GPS("LONGITDUE LIMIT PASS");
float small = 0.0001;
if(bestPos.longitudeStdDev > small && bestPos.latitudeStdDev > small)
{
LOG.GPS("Std are not equal to 0 PASS");
// Set up the matrix
sensor->H = Matrix_Create(2,6);
sensor->H = Matrix_SetValue(sensor->H,1,1,1);
sensor->H = Matrix_SetValue(sensor->H,2,2,1);
matrix Variance = Matrix_Create(2,2);
sensor->state->Variance=Variance;
matrix Location = Matrix_Create(2,1);
sensor->state->State = Location;
if(bestPos.differentialAge > 10.0)
{
Variance = Matrix_SetValue(Variance,1,1,bestPos.latitudeStdDev * bestPos.latitudeStdDev + 4);
Variance = Matrix_SetValue(Variance,2,2,bestPos.longitudeStdDev * bestPos.longitudeStdDev + 4);
}
else
{
Variance = Matrix_SetValue(Variance,1,1,bestPos.latitudeStdDev * bestPos.latitudeStdDev + 2.);
Variance = Matrix_SetValue(Variance,2,2,bestPos.longitudeStdDev * bestPos.longitudeStdDev + 2.);
}
sensor->state->Variance=Variance;
double newLat = (bestPos.latitude - GPS_latOffset) * GPS_LatitudeRatio;
double newLong = (bestPos.longitude-GPS_longOffset) * GPS_LongitudeRatio;
// Stores the longitude and the latitude readings
sensor->state->State = Matrix_SetValue(sensor->state->State,1,1,(float)newLat - Matrix_GetX(state.State));
sensor->state->State = Matrix_SetValue(sensor->state->State,2,1,(float)newLong - Matrix_GetY(state.State));
LOG.GPS("GPS meter in x = %10.20lf",newLat);
LOG.GPS("GPS meter in y = %10.20lf",newLong);
LOG.GPS("GPS meter in x stdDev = %f",bestPos.latitudeStdDev);
LOG.GPS("GPS meter in y stdDev = %f",bestPos.longitudeStdDev);
LOG.DATA("GPS meter in x = %10.20lf",newLat);
LOG.DATA("GPS meter in y = %10.20lf",newLong);
LOG.DATA("GPS meter in x stdDev = %f",bestPos.latitudeStdDev);
LOG.DATA("GPS meter in y stdDev = %f",bestPos.longitudeStdDev);
sensor->hasBeenUpdated = 1;
LOG.GPS("GPS used");
return;
}
}
}
}
else
{
LOG.GPS("UsedSatelites Failed");
}
}
else
{
LOG.GPS("Position type was no good: %u", bestPos.posType);
}
}
else
{
LOG.GPS("Solution Status not 'Computed'. It was: %u", bestPos.solStat);
}
}
}
/*
* This is a c implenetation of a template made my Mike Dinolfo in 1998. Highly appriciated
*/
matrix Matrix_Inverse(matrix A)
{
int actualsize = A.rows;
int maxsize = A.columns;
MATRIXLIBDATATYPE * data = &A.data[0];
if (actualsize <= 0)
{
LOG.ERR("The size of the Matrix was less than 0");
return A; /* sanity check*/
}
if (actualsize == 1)
{
/* if its a one by one then just inverse the value */
matrix out = Matrix_Create(1,1);
out = Matrix_SetValue(out,1,1,((MATRIXLIBDATATYPE)1.0)/(Matrix_GetValue(A,1,1)));
return out; /* must be of dimension >= 2*/
}
int i;
int j;
int k;
for (i = 1; i < actualsize; i++) data[i] /= data[0]; /* normalize row 0*/
for (i = 1; i < actualsize; i++) {
for (j = i; j < actualsize; j++) { /* do a column of L*/
MATRIXLIBDATATYPE sum = 0.0;
for (k = 0; k < i; k++)
sum += data[j * maxsize + k] * data[k * maxsize + i];
data[j * maxsize + i] -= sum;
}
if (i == actualsize - 1) continue;
for (j = i + 1; j < actualsize; j++) { /* do a row of U*/
MATRIXLIBDATATYPE sum = 0.0;
for (k = 0; k < i; k++)
sum += data[i * maxsize + k] * data[k * maxsize + j];
data[i * maxsize + j] =
(data[i * maxsize + j] - sum) / data[i * maxsize + i];
}
}
for (i = 0; i < actualsize; i++) /* invert L*/
for (j = i; j < actualsize; j++) {
MATRIXLIBDATATYPE x = 1.0;
if (i != j) {
x = 0.0;
for (k = i; k < j; k++)
x -= data[j * maxsize + k] * data[k * maxsize + i];
}
data[j * maxsize + i] = x / data[j * maxsize + j];
}
for (i = 0; i < actualsize; i++) /* invert U*/
for (j = i; j < actualsize; j++) {
if (i == j) continue;
MATRIXLIBDATATYPE sum = 0.0;
for (k = i; k < j; k++)
sum += data[k * maxsize + j]*((i == k) ? 1.0 : data[i * maxsize + k]);
data[i * maxsize + j] = -sum;
}
for (i = 0; i < actualsize; i++) /* final inversion*/
for (j = 0; j < actualsize; j++) {
MATRIXLIBDATATYPE sum = 0.0;
for (k = ((i > j) ? i : j); k < actualsize; k++)
sum += ((j == k) ? 1.0 : data[j * maxsize + k]) * data[k * maxsize + i];
data[j * maxsize + i] = sum;
}
return A;
}
boolean PartLib_Create (char *DbName, char *DbFileName,
dbaccesstype DbAccess, boolean UseVparts,
numtype HndlRgns, numtype RgnHndls,
numtype VhndlRgns, numtype RgnVhndls,
numtype XmemRgns, numtype RgnXmems,
ft F,lt Z,zz *Status,
tokentype *PrimalTkn, PartLib **PrimalObj)
{
addrtype IntVarray = NullPtr;
numtype GrpNum = 0;
grpheader *GrpHdr = NullPtr;
sizetype EntrySize = sizeof (parttkntype);
numtype AllocRgns = (RgnHndls / 125) + 27;
numtype RgnEntrys = 125;
numtype AllocXs = 1000;
numtype BaseIndex = 2;
accesstype AccessType = Read_Write;
boolean DiskCache = False;
numtype CacheLimit = AllocRgns;
numtype CacheLoad = AllocRgns;
numtype NumBuffRgns = AllocRgns;
numtype BaseRegion = 0;
parttkntype PartToken;
indextype Index = 0;
boolean TempClassBug = ClassBug;
/* ClassBug = True; */
VlinksDir = 0;
if (ClassBug)
if (sprintf(Msg, " PartLib_Create:: Db= '%32s'; AllocRgns=%6u\n",
DbName, AllocRgns))
TraceMsg (0, Msg);
Primal_CreateDb ("PartSchema", DbName, DbFileName, DbAccess,
HndlRgns, RgnHndls, VhndlRgns, RgnVhndls,
XmemRgns, RgnXmems, McStat, PrimalTkn);
OaDumpObject( 0, PrimalTkn, McStat ) ;
if (Normal(*Status))
{
if (Object_GetImage(PrimalTkn, McStat, (addrtype *)PrimalObj))
if (Grp_NewPacket (PrimalTkn, EntrySize, AllocRgns, RgnEntrys,
AllocXs, BaseIndex, AccessType, DiskCache,
CacheLimit,CacheLoad, NumBuffRgns,
McStat, &GrpNum, &GrpHdr, &BaseRegion))
if (Grp_NewEntry (PrimalTkn, GrpNum,
McStat, &Index, (addrtype )&PartToken))
{
GrpHdr->EnvObjType = EnvType_AttrNum;
VlinksDir = 0;
}
#ifdef __INCLUDE_MTRX__
if (Normal(*Status))
{
MultiMate = False;
AllocMates = 30000;
AllocPoints = 0;
RgnEntrys = 1000;
AllocRgns = 10;
AllocXs = 1000;
BaseIndex = 0;
AccessType = Read_Write;
DiskCache = False;
CacheLimit = 10;
CacheLoad = 5;
NumBuffRgns = 10;
MaxBuckets = 2000;
Divisor = 10;
KeyType = aInteger;
KeySize = sizeof (partidtype);
Matrix_Create (TosFroms_Mtrx, PrimalTkn,
AllocKeys, MaxBuckets, Divisor,
AllocKeys, MaxBuckets, Divisor,
AllocMates, AllocPoints,
AllocRgns, RgnEntrys, AllocXs,
DiskCache, CacheLimit, CacheLoad,
NumBuffRgns, McStat, &MtrxTkn);
}
#endif /* __INCLUDE_MTRX__ */
if (Normal(*Status))
if (UseVparts)
{
if (Attr_ArrayCreate (PrimalTkn, PartsListHead_Attr,
AllocParts, sizeof(int),
McStat, &IntVarray))
{
VlistTkn.DbId = PrimalTkn->DbId;
VlistTkn = (*PrimalObj)->PartsListHead.VchunkTkn;
NumVlists = 0;
VchunkPutStackPtr (&VlistTkn, 0,
McStat, (addrtype *)&Vlists);
}
}
if (Normal(*Status))
if (UseVparts)
{
(*PrimalObj)->VpartsDir = GrpNum;
VpartsDir = GrpNum;
}
if (sprintf(Msg, " PartLibCreate:: Db[%3u]; VpartsDir=%4u\n",
PrimalTkn->DbId, VpartsDir))
TraceMsg (0, Msg);
if (Normal(*Status))
if (Object_CommitImage (PrimalTkn, False, McStat))
{
Print_Msg = True;
WriteHdr = False;
}
} else if (*Status == Csh_DbAlreadyExists) {
*Status = Env_Normal;
if (Object_GetImage (PrimalTkn, McStat, (addrtype *)PrimalObj))
if (UseVparts)
if ((*PrimalObj)->VpartsDir == 0)
{
if (Grp_NewPacket (PrimalTkn, EntrySize, AllocRgns, RgnEntrys,
AllocXs, BaseIndex, AccessType, DiskCache,
CacheLimit,CacheLoad, NumBuffRgns,
McStat, &GrpNum, &GrpHdr, &BaseRegion))
if (Grp_NewEntry (PrimalTkn, GrpNum,
McStat, &Index, (addrtype )&PartToken))
{
GrpHdr->EnvObjType = EnvType_AttrNum;
(*PrimalObj)->VpartsDir = GrpNum;
VpartsDir = GrpNum;
Object_CommitImage(PrimalTkn, False, McStat);
}
}
if (Normal(*Status))
if ((*PrimalObj)->VpartsDir)
if (Grp_GetPacket (PrimalTkn, (*PrimalObj)->VpartsDir,
McStat, &GrpHdr))
VpartsDir = (*PrimalObj)->VpartsDir;
if (Normal(*Status))
{
VlistTkn = (*PrimalObj)->PartsListHead.VchunkTkn;
if (VlistTkn.Handle)
if (Vchunk_GetStackPtr(&VlistTkn, McStat, &NumVlists))
Vchunk_GetAddress (&VlistTkn, McStat, (addrtype *)&Vlists);
}
}
if (Normal(*Status))
if (ClassBug || DeBug >= 0)
Object_Dump (PrimalTkn);
ClassBug = TempClassBug;
TRACK(TrackBak,"PartLib_Create\n");
return (STAT);
}