int CCONV gps_on_position_change(CPhidgetGPSHandle gps, void *userptr, double latitude, double longitude, double altitude) {
PhidgetInfo *info = userptr;
GpsInfo *gps_info = info->type_info;
// Calculate the sample time:
sample_tick(gps_info->sample_rate, NULL);
// Get the GPS time:
GPSDate date;
GPSTime time;
if ( (CPhidgetGPS_getDate(gps, &date) == EPHIDGET_OK) &&
(CPhidgetGPS_getTime(gps, &time) == EPHIDGET_OK) ) {
gps_info->now_at_utc.tm_sec = time.tm_sec;
gps_info->now_at_utc.tm_min = time.tm_min;
gps_info->now_at_utc.tm_hour = time.tm_hour;
gps_info->now_at_utc.tm_mday = date.tm_mday;
gps_info->now_at_utc.tm_mon = date.tm_mon;
gps_info->now_at_utc.tm_year = date.tm_year;
gps_info->now_at_utc_ms = (time.tm_ms >= 1000) ? 0 : time.tm_ms;
gps_info->is_now_at_utc_known = true;
} else
gps_info->is_now_at_utc_known = false;
// Get Position values:
if (gps_info->latitude != PUNK_DBL) {
gps_info->is_latitude_known = true;
gps_info->latitude = latitude;
} else
gps_info->is_latitude_known = false;
if (gps_info->longitude != PUNK_DBL) {
gps_info->is_longitude_known = true;
gps_info->longitude = longitude;
} else
gps_info->is_longitude_known = false;
if (gps_info->altitude != PUNK_DBL) {
gps_info->is_altitude_known = true;
gps_info->altitude = altitude;
} else
gps_info->is_altitude_known = false;
double heading, velocity;
if (CPhidgetGPS_getHeading(gps, &heading) == EPHIDGET_OK) {
gps_info->heading = heading;
gps_info->is_heading_known = true;
} else
gps_info->is_heading_known = false;
if (CPhidgetGPS_getVelocity(gps, &velocity) == EPHIDGET_OK) {
gps_info->velocity = velocity;
gps_info->is_velocity_known = true;
} else
gps_info->is_velocity_known = false;
return 0;
}
int CCONV gps_on_fix_change(CPhidgetGPSHandle gps, void *userptr, int status) {
PhidgetInfo *info = userptr;
GpsInfo *gps_info = info->type_info;
// Calculate the sample time:
sample_tick(gps_info->sample_rate, NULL);
// I'm fairly certain that status is always either 1 or 0
gps_info->is_fixed = (status) ? true : false;
return 0;
}
//callback that will run at datarate
//data - array of spatial event data structures that holds the spatial data packets that were sent in this event
//count - the number of spatial data event packets included in this event
int CCONV spatial_on_data(CPhidgetSpatialHandle spatial, void *userptr, CPhidgetSpatial_SpatialEventDataHandle *data, int count)
{
PhidgetInfo *info = userptr;
SpatialInfo *spatial_info = info->type_info;
int i;
for(i = 0; i < count; i++) {
sample_tick(spatial_info->sample_rate, &data[i]->timestamp);
// Set the values to where they need to be:
for(int j=0; j < spatial_info->accelerometer_axes; j++)
spatial_info->acceleration[j] = data[i]->acceleration[j];
spatial_info->is_acceleration_known = true;
// Sometimes the compass will return nonesense in the form of this constant
// I'm fairly certain that simply checking the first element of the array
// will suffice, and when this is the case, we just keep the prior values:
if (data[i]->magneticField[0] == PUNK_DBL)
spatial_info->is_compass_known = false;
else {
for(int j=0; j < spatial_info->compass_axes; j++)
spatial_info->compass[j] = data[i]->magneticField[j];
spatial_info->is_compass_known = true;
}
// Gyro Time! these get handled slightly different...
double timestamp = data[i]->timestamp.seconds + data[i]->timestamp.microseconds/MICROSECONDS_IN_SECOND;
double timechange = (spatial_info->last_microsecond > 0) ?
timestamp - spatial_info->last_microsecond : 0;
// This is kind of superceeded by the orientation_ functions, but I'm
// leaving it in for posterity:
for(int j=0; j < spatial_info->gyro_axes; j++)
spatial_info->gyroscope[j] += data[i]->angularRate[j] * timechange;
spatial_info->is_gyroscope_known = true;
// If we're dealing with 3 dimensional sensors, we can calculate orientation:
if (spatial_info->gyro_axes == 3) {
// This is a shortcut to keep things DRY below:
double angular_rate_in_rad[3];
for(int j=0; j < 3; j++)
angular_rate_in_rad[j] = data[i]->angularRate[j] * M_PI/180;
euler_to_3x3dcm((double *) &spatial_info->gyroscope_dcm,
angular_rate_in_rad[0] * timechange,
angular_rate_in_rad[1] * timechange,
angular_rate_in_rad[2] * timechange,
NULL);
// Madgwick Orientation time - Update the ahrs:
if (!spatial_info->is_ahrs_initialized) {
if (spatial_info->is_compass_known)
spatial_ahrs_init(spatial_info,
data[i]->acceleration[0],
data[i]->acceleration[1],
data[i]->acceleration[2],
data[i]->magneticField[0],
data[i]->magneticField[1],
data[i]->magneticField[2]);
} else {
float gx = (float) angular_rate_in_rad[0];
float gy = (float) angular_rate_in_rad[1];
float gz = (float) angular_rate_in_rad[2];
float ax = (float) data[i]->acceleration[0];
float ay = (float) data[i]->acceleration[1];
float az = (float) data[i]->acceleration[2];
if (spatial_info->is_compass_known)
// We have enough info for a MARG update:
spatial_ahrs_update(spatial_info, gx, gy, gz, ax, ay, az,
(float) data[i]->magneticField[0],
(float) data[i]->magneticField[1],
(float) data[i]->magneticField[2]);
else
// We only have enough info for a IMU update:
spatial_ahrs_update_imu(spatial_info, gx, gy, gz, ax, ay, az);
}
}
// We'll need this on the next go around:
spatial_info->last_microsecond = timestamp;
}
return 0;
}
static void xpp_drift_step(xbus_t *xbus, const struct timeval *tv)
{
struct xpp_drift *driftinfo = &xbus->drift;
struct xpp_ticker *ticker = &xbus->ticker;
unsigned long flags;
bool cycled;
spin_lock_irqsave(&driftinfo->lock, flags);
cycled = xpp_ticker_step(&xbus->ticker, tv);
if(ref_ticker && syncer && xbus->sync_mode == SYNC_MODE_PLL) {
int new_delta_tick = ticker->count - ref_ticker->count;
int lost_ticks = new_delta_tick - driftinfo->delta_tick;
driftinfo->delta_tick = new_delta_tick;
if(lost_ticks) {
driftinfo->lost_ticks++;
driftinfo->lost_tick_count += abs(lost_ticks);
XBUS_DBG(SYNC, xbus, "Lost %d tick%s\n",
lost_ticks,
(abs(lost_ticks) > 1) ? "s": "");
ticker->cycle = SYNC_ADJ_QUICK;
if(abs(lost_ticks) > 100)
ticker->count = ref_ticker->count;
} else {
long usec_delta;
bool nofix = 0;
usec_delta = (long)usec_diff(
&ticker->last_sample.tv,
&ref_ticker->last_sample.tv);
usec_delta -= driftinfo->wanted_offset;
sample_tick(xbus, usec_delta);
if(abs(usec_delta) > 300) {
/*
* We are close to the edge, send a brutal
* fix, and skip calculation until next time.
*/
if(usec_delta > 0 && xbus->sync_adjustment > -SYNC_ADJ_MAX) {
XBUS_DBG(SYNC, xbus, "Pullback usec_delta=%ld\n", usec_delta);
send_drift(xbus, -SYNC_ADJ_MAX); /* emergency push */
}
if(usec_delta < 0 && xbus->sync_adjustment < SYNC_ADJ_MAX) {
XBUS_DBG(SYNC, xbus, "Pushback usec_delta=%ld\n", usec_delta);
send_drift(xbus, SYNC_ADJ_MAX); /* emergency push */
}
ticker->cycle = SYNC_ADJ_QUICK;
nofix = 1;
} else {
/* good data, use it */
if(usec_delta > driftinfo->delta_max)
driftinfo->delta_max = usec_delta;
if(usec_delta < driftinfo->delta_min)
driftinfo->delta_min = usec_delta;
}
if(!nofix && cycled) {
int offset = 0;
driftinfo->median = (driftinfo->delta_max + driftinfo->delta_min) / 2;
driftinfo->jitter = driftinfo->delta_max - driftinfo->delta_min;
if(abs(driftinfo->median) >= 150) { /* more than 1 usb uframe */
int factor = abs(driftinfo->median) / 125;
factor = 1 + (factor * 8000) / ticker->cycle;
if(driftinfo->median > 0)
offset = driftinfo->calc_drift - factor;
else
offset = driftinfo->calc_drift + factor;
/* for large median, push some more */
if(abs(driftinfo->median) >= 300) { /* more than 2 usb uframes */
ticker->cycle = SYNC_ADJ_QUICK;
XBUS_NOTICE(xbus,
"Back to quick: median=%d\n",
driftinfo->median);
}
} else {
ticker->cycle += 500;
if(ticker->cycle >= SYNC_ADJ_SLOW)
ticker->cycle = SYNC_ADJ_SLOW;
}
driftinfo->calc_drift = offset;
XBUS_DBG(SYNC, xbus,
"ADJ: min=%d max=%d jitter=%d median=%d offset=%d\n",
driftinfo->delta_min,
driftinfo->delta_max,
driftinfo->jitter,
driftinfo->median,
offset);
if(offset < -SYNC_ADJ_MAX)
offset = -SYNC_ADJ_MAX;
if(offset > SYNC_ADJ_MAX)
offset = SYNC_ADJ_MAX;
xbus->sync_adjustment_offset = offset;
if(xbus != syncer && xbus->sync_adjustment != offset)
send_drift(xbus, offset);
driftinfo_recalc(driftinfo);
}
}
}
spin_unlock_irqrestore(&driftinfo->lock, flags);
}
