bool AP_InertialSensor_QURT::update(void)
{
accumulate();
update_accel(accel_instance);
update_gyro(gyro_instance);
return true;
}
bool AP_InertialSensor_PX4::update(void)
{
// get the latest sample from the sensor drivers
_get_sample();
for (uint8_t k=0; k<_num_accel_instances; k++) {
update_accel(_accel_instance[k]);
}
for (uint8_t k=0; k<_num_gyro_instances; k++) {
update_gyro(_gyro_instance[k]);
}
return true;
}
/**
* Applies a Transform's inverse.
*/
void reverse_transform(const Transform &t) {
// Origin and direction
o = t.pos_from(o);
d = t.dir_from(d);
// Differentials
// These can be transformed as directional vectors...?
/*if (has_differentials) {
odx = t.dir_from(odx);
ody = t.dir_from(ody);
ddx = t.dir_from(ddx);
ddy = t.dir_from(ddy);
}*/
update_accel();
//update_differentials();
}
/*
* Finalizes the ray after first initialization.
* Should only be called once, prior to tracing with the ray.
*/
void finalize() {
// TODO: will normalizing things here mess anything up elsewhere?
//assert(d.length() > 0.0f);
float linv = 1.0f / d.length();
d.normalize();
// Adjust the ray differentials for the normalized ray
dw *= linv;
/*if (has_differentials) {
odx *= linv;
ody *= linv;
ddx *= linv;
ddy *= linv;
}*/
update_accel();
//update_differentials();
}
