void motorsReceiveTelem(uint8_t canId, uint8_t doc, void *p) {
uint32_t *data = (uint32_t *)p;
uint32_t *storage = (uint32_t *)&motorsData.canStatus[canId-1];
uint32_t micros = timerMicros();
#ifdef MOTORS_CAN_LOGGING
motorsLog_t *buf = (motorsLog_t *)(motorsLogBuf + motorsData.head);
buf->sync = 0xff;
buf->escId = 0xc0 | canId; // 2 MSB are high as part of sync bits
buf->micros = micros;
buf->data[0] = data[0];
buf->data[1] = data[1];
motorsData.head = (motorsData.head + sizeof(motorsLog_t)) % sizeof(motorsLogBuf);
filerSetHead(motorsData.logHandle, motorsData.head);
#endif
// copy status data to our storage (8 bytes)
storage[0] = data[0];
storage[1] = data[1];
// record reception time
motorsData.canStatusTime[canId-1] = micros;
}
void loggerDo(void) {
int32_t head;
char *buf;
char ckA, ckB;
int i;
// make sure we can proceed
if (!filerAvailable())
return;
head = filerGetHead(loggerData.logHandle);
buf = loggerData.loggerBuf + head;
// log header signature
*buf++ = 'A';
*buf++ = 'q';
*buf++ = 'M';
// number of fields
for (i = 0; i < loggerData.numFields; i++)
buf += loggerData.fp[i].copyFunc(buf, loggerData.fp[i].fieldPointer);
ckA = ckB = 0;
buf = loggerData.loggerBuf + head;
for (i = 3; i < loggerData.packetSize - 2; i++) {
ckA += buf[i];
ckB += ckA;
}
buf[i++] = ckA;
buf[i++] = ckB;
filerSetHead(loggerData.logHandle, (head + loggerData.packetSize) % loggerData.bufSize);
}
void navUkfGpsVelUpdate(uint32_t gpsMicros, float velN, float velE, float velD, float sAcc) {
float y[3];
float noise[3];
float velDelta[3];
int histIndex;
y[0] = velN;
y[1] = velE;
y[2] = velD;
// determine how far back this GPS velocity update came from
histIndex = (timerMicros() - (gpsMicros + UKF_VEL_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP);
histIndex = navUkfData.navHistIndex - histIndex;
if (histIndex < 0)
histIndex += UKF_HIST;
if (histIndex < 0 || histIndex >= UKF_HIST)
histIndex = 0;
// calculate delta from current position
velDelta[0] = UKF_VELN - navUkfData.velN[histIndex];
velDelta[1] = UKF_VELE - navUkfData.velE[histIndex];
velDelta[2] = UKF_VELD - navUkfData.velD[histIndex];
// set current position state to historic data
UKF_VELN = navUkfData.velN[histIndex];
UKF_VELE = navUkfData.velE[histIndex];
UKF_VELD = navUkfData.velD[histIndex];
noise[0] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_VEL_M_N;
noise[1] = UKF_GPS_VEL_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_VEL_M_N;
noise[2] = UKF_GPS_VD_N + sAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_VD_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfVelUpdate);
// add the historic position delta back to the current state
UKF_VELN += velDelta[0];
UKF_VELE += velDelta[1];
UKF_VELD += velDelta[2];
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = y[0];
log[i++] = y[1];
log[i++] = y[2];
log[i++] = noise[0];
log[i++] = noise[1];
log[i++] = noise[2];
log[i++] = velDelta[0];
log[i++] = velDelta[1];
log[i++] = velDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
void loggerDoHeader(void) {
int32_t head;
char *buf;
char ckA, ckB;
int i;
// make sure we can proceed
if (!filerAvailable())
return;
head = filerGetHead(loggerData.logHandle);
buf = loggerData.loggerBuf + head;
// log header signature
*buf++ = 'A';
*buf++ = 'q';
*buf++ = 'H';
// number of fields
*buf++ = loggerData.numFields;
// fields and types
memcpy(buf, loggerFields, sizeof(loggerFields));
// calc checksum
ckA = ckB = 0;
buf = loggerData.loggerBuf + head + 3;
for (i = 0; i < (1 + sizeof(loggerFields)); i++) {
ckA += buf[i];
ckB += ckA;
}
buf[i++] = ckA;
buf[i++] = ckB;
// block size is the actual data packet size
filerSetHead(loggerData.logHandle, (head + loggerData.packetSize) % loggerData.bufSize);
}
/*
We take raw pixel movement data reported by the sensor and combine
it with our own estimates of ground height and rotational rates
to calculate the x/y velocity estimates. Along with reported sonar
altitudes, they are fed to the UKF as observations.
*/
void navUkfFlowUpdate(void) {
static float oldPitch, oldRoll;
float flowX, flowY;
float xT, yT;
float dt;
float flowAlt = 0.0f;
float y[3];
float noise[3];
// set default noise levels
noise[0] = 0.001f;
noise[1] = 0.001f;
noise[2] = 100.0f;
// valid altitudes?
if (navUkfData.flowAltCount > 0) {
flowAlt = navUkfData.flowSumAlt / navUkfData.flowAltCount;
if (fabsf(navUkfData.flowAlt - flowAlt) < 1.0f)
noise[2] = 0.0025f;
navUkfData.flowAlt = flowAlt;
}
// first valid flow update ?
if (navUkfData.flowInit == 0) {
// only allow init if we have a valid altitude
if (navUkfData.flowAltCount > 0) {
UKFPressureAdjust(navUkfData.flowAlt);
UKF_POSD = navUkfData.flowAlt;
navUkfData.flowInit = 1;
}
}
else {
// scaled, average quality
navUkfData.flowQuality = (float)navUkfData.flowSumQuality / navUkfData.flowCount * (1.0f / 255.0f);
// first rotate sensor data to craft frame around Z axis
flowX = navUkfData.flowSumX * navUkfData.flowRotCos + navUkfData.flowSumY * navUkfData.flowRotSin;
flowY = navUkfData.flowSumY * navUkfData.flowRotCos - navUkfData.flowSumX * navUkfData.flowRotSin;
// adjust for pitch/roll rotations during measurement
flowX -= (AQ_PITCH - oldPitch) * DEG_TO_RAD * UKF_FOCAL_PX;
flowY += (AQ_ROLL - oldRoll) * DEG_TO_RAD * UKF_FOCAL_PX;
// next, rotate flow to world frame
xT = flowX * navUkfData.yawCos - flowY * navUkfData.yawSin;
yT = flowY * navUkfData.yawCos + flowX * navUkfData.yawSin;
// convert to distance covered based on focal length and height above ground
flowX = xT * (1.0f / UKF_FOCAL_PX) * UKF_POSD;
flowY = yT * (1.0f / UKF_FOCAL_PX) * UKF_POSD;
// integrate for absolute position
navUkfData.flowPosN += flowX;
navUkfData.flowPosE += flowY;
// time delta - assume each count is from two readings @ 5ms each
dt = (float)navUkfData.flowCount * (5.0f * 2.0f) / 1000.0f;
// differentiate for velocity
navUkfData.flowVelX = flowX / dt;
navUkfData.flowVelY = flowY / dt;
// TODO: properly estimate noise
noise[0] = (UKF_GPS_POS_N + UKF_GPS_POS_M_N) * 0.5f;
noise[1] = noise[0];
navUkfCalcLocalDistance(navUkfData.flowPosN, navUkfData.flowPosE, &y[0], &y[1]);
y[2] = navUkfData.flowAlt;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfOfPosUpdate);
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = flowX;
log[i++] = flowY;
log[i++] = navUkfData.flowVelX;
log[i++] = navUkfData.flowVelY;
log[i++] = navUkfData.flowPosN;
log[i++] = navUkfData.flowPosE;
log[i++] = flowAlt;
log[i++] = UKF_POSN;
log[i++] = UKF_POSE;
log[i++] = UKF_POSD;
log[i++] = UKF_PRES_ALT;
log[i++] = UKF_VELD;
log[i++] = navUkfData.flowQuality;
log[i++] = (AQ_PITCH - oldPitch);
log[i++] = (AQ_ROLL - oldRoll);
log[i++] = dt;
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
navUkfData.flowSumX = 0.0f;
navUkfData.flowSumY = 0.0f;
navUkfData.flowSumQuality = 0;
navUkfData.flowSumAlt = 0.0f;
navUkfData.flowCount = 0;
navUkfData.flowAltCount = 0;
oldRoll = AQ_ROLL;
oldPitch = AQ_PITCH;
}
void navUkfGpsPosUpdate(uint32_t gpsMicros, double lat, double lon, float alt, float hAcc, float vAcc) {
float y[3];
float noise[3];
float posDelta[3];
int histIndex;
if (navUkfData.holdLat == (double)0.0) {
navUkfData.holdLat = lat;
navUkfData.holdLon = lon;
navUkfCalcEarthRadius(lat);
navUkfSetGlobalPositionTarget(lat, lon);
navUkfResetPosition(-UKF_POSN, -UKF_POSE, alt - UKF_POSD);
}
else {
navUkfCalcGlobalDistance(lat, lon, &y[0], &y[1]);
y[2] = alt;
// determine how far back this GPS position update came from
histIndex = (timerMicros() - (gpsMicros + UKF_POS_DELAY)) / (int)(1e6f * AQ_OUTER_TIMESTEP);
histIndex = navUkfData.navHistIndex - histIndex;
if (histIndex < 0)
histIndex += UKF_HIST;
if (histIndex < 0 || histIndex >= UKF_HIST)
histIndex = 0;
// calculate delta from current position
posDelta[0] = UKF_POSN - navUkfData.posN[histIndex];
posDelta[1] = UKF_POSE - navUkfData.posE[histIndex];
posDelta[2] = UKF_POSD - navUkfData.posD[histIndex];
// set current position state to historic data
UKF_POSN = navUkfData.posN[histIndex];
UKF_POSE = navUkfData.posE[histIndex];
UKF_POSD = navUkfData.posD[histIndex];
noise[0] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.nDOP*gpsData.nDOP) * UKF_GPS_POS_M_N;
noise[1] = UKF_GPS_POS_N + hAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.eDOP*gpsData.eDOP) * UKF_GPS_POS_M_N;
noise[2] = UKF_GPS_ALT_N + vAcc * __sqrtf(gpsData.tDOP*gpsData.tDOP + gpsData.vDOP*gpsData.vDOP) * UKF_GPS_ALT_M_N;
srcdkfMeasurementUpdate(navUkfData.kf, 0, y, 3, 3, noise, navUkfPosUpdate);
// add the historic position delta back to the current state
UKF_POSN += posDelta[0];
UKF_POSE += posDelta[1];
UKF_POSD += posDelta[2];
#ifdef UKF_LOG_FNAME
{
float *log = (float *)&ukfLog[navUkfData.logPointer];
int i = 0;
*(uint32_t *)&log[i++] = 0xffffffff;
log[i++] = y[0];
log[i++] = y[1];
log[i++] = y[2];
log[i++] = noise[0];
log[i++] = noise[1];
log[i++] = noise[2];
log[i++] = posDelta[0];
log[i++] = posDelta[1];
log[i++] = posDelta[2];
navUkfData.logPointer = (navUkfData.logPointer + UKF_LOG_SIZE) % UKF_LOG_BUF_SIZE;
filerSetHead(navUkfData.logHandle, navUkfData.logPointer);
}
#endif
}
}
