int HMC5883::check_calibration()
{
bool offset_valid = (check_offset() == OK);
bool scale_valid = (check_scale() == OK);
if (_calibrated != (offset_valid && scale_valid)) {
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
// XXX Change advertisement
/* notify about state change */
struct subsystem_info_s info = {
true,
true,
_calibrated,
SUBSYSTEM_TYPE_MAG};
static orb_advert_t pub = -1;
if (pub > 0) {
orb_publish(ORB_ID(subsystem_info), pub, &info);
} else {
pub = orb_advertise(ORB_ID(subsystem_info), &info);
}
}
/* return 0 if calibrated, 1 else */
return (!_calibrated);
}
inline bool check_weibull(
const char* function,
RealType scale,
RealType shape,
RealType* result, const Policy& pol)
{
return check_scale(function, scale, result, pol) && check_weibull_shape(function, shape, result, pol);
}
inline bool check_inverse_gamma(
const char* function, // TODO swap these over, so shape is first.
RealType scale, // scale aka beta
RealType shape, // shape aka alpha
RealType* result, const Policy& pol)
{
return check_scale(function, scale, result, pol)
&& check_inverse_gamma_shape(function, shape, result, pol);
} // bool check_inverse_gamma
inline bool check_inverse_chi_squared( // Check both distribution parameters.
const char* function,
RealType degrees_of_freedom, // degrees_of_freedom (aka nu).
RealType scale, // scale (aka sigma^2)
RealType* result,
const Policy& pol)
{
return check_scale(function, scale, result, pol)
&& check_df(function, degrees_of_freedom,
result, pol);
} // bool check_inverse_chi_squared
int HMC5883::check_calibration()
{
bool offset_valid = (check_offset() == OK);
bool scale_valid = (check_scale() == OK);
if (_calibrated != (offset_valid && scale_valid)) {
_calibrated = (offset_valid && scale_valid);
}
/* return 0 if calibrated, 1 else */
return (!_calibrated);
}
int LIS3MDL::check_calibration()
{
bool offset_valid = (check_offset() == OK);
bool scale_valid = (check_scale() == OK);
if (_calibrated != (offset_valid && scale_valid)) {
warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
_calibrated = (offset_valid && scale_valid);
}
/* return 0 if calibrated, 1 else */
return !_calibrated;
}
int IST8310::check_calibration()
{
bool offset_valid = (check_offset() == OK);
bool scale_valid = (check_scale() == OK);
if (_calibrated != (offset_valid && scale_valid)) {
if (!scale_valid || !offset_valid) {
PX4_WARN("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ",
(offset_valid) ? "" : "offset invalid");
}
_calibrated = (offset_valid && scale_valid);
}
/* return 0 if calibrated, 1 else */
return (!_calibrated);
}
void toggle_scale_mode (GtkWidget *widget, gpointer user_data)
{
print_properties_D *dialog = (print_properties_D *)gtk_object_get_data (GTK_OBJECT (user_data), "dialog") ;
gboolean bAuto = gtk_toggle_button_get_active (GTK_TOGGLE_BUTTON (dialog->rbFitPages)) ;
if (NULL != widget)
if (!gtk_toggle_button_get_active (GTK_TOGGLE_BUTTON (widget))) return ;
gtk_widget_set_sensitive (dialog->fmScale, !bAuto) ;
gtk_widget_set_sensitive (dialog->lblNanoIs, !bAuto) ;
gtk_widget_set_sensitive (dialog->spnNanoToUnits, !bAuto) ;
gtk_widget_set_sensitive (dialog->lblScale, !bAuto) ;
gtk_widget_set_sensitive (dialog->fmFit, bAuto) ;
gtk_widget_set_sensitive (dialog->spnCXPages, bAuto) ;
gtk_widget_set_sensitive (dialog->spnCYPages, bAuto) ;
gtk_widget_set_sensitive (dialog->lblPgsWide, bAuto) ;
gtk_widget_set_sensitive (dialog->lblPgsTall, bAuto) ;
check_scale (dialog, NULL) ;
}
void chkPrintedObj_toggled (GtkWidget *widget, gpointer user_data)
{
QCADLayer *layer = NULL ;
print_properties_D *dialog = (print_properties_D *)gtk_object_get_data (GTK_OBJECT (user_data), "dialog") ;
int cx = -1, cy = -1 ;
if (!gtk_toggle_button_get_active (GTK_TOGGLE_BUTTON (widget)))
{
calc_world_size (&cx, &cy, dialog) ;
if (0 == cx || 0 == cy)
{
gtk_toggle_button_set_active (GTK_TOGGLE_BUTTON (widget), TRUE) ;
gdk_beep () ;
return ;
}
}
if (NULL != (layer = g_object_get_data (G_OBJECT (widget), "layer")))
g_object_set_data (G_OBJECT (layer), "print_layer", (gpointer)gtk_toggle_button_get_active (GTK_TOGGLE_BUTTON (widget))) ;
check_scale (dialog, NULL) ;
}
int HMC5883::calibrate(struct file *filp, unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret = 1;
// XXX do something smarter here
int fd = (int)enable;
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
float avg_excited[3] = {0.0f, 0.0f, 0.0f};
unsigned i;
warnx("starting mag scale calibration");
/* do a simple demand read */
sz = read(filp, (char *)&report, sizeof(report));
if (sz != sizeof(report)) {
warn("immediate read failed");
ret = 1;
goto out;
}
warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
warnx("time: %lld", report.timestamp);
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
warn("failed to set queue depth");
ret = 1;
goto out;
}
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
ret = 1;
goto out;
}
/* Set to 2.5 Gauss */
if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
ret = 1;
goto out;
}
/* read the sensor 10x and report each value */
for (i = 0; i < 500; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
goto out;
} else {
avg_excited[0] += report.x;
avg_excited[1] += report.y;
avg_excited[2] += report.z;
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
}
avg_excited[0] /= i;
avg_excited[1] /= i;
avg_excited[2] /= i;
warnx("done. Performed %u reads", i);
warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
float scaling[3];
/* calculate axis scaling */
scaling[0] = fabsf(1.16f / avg_excited[0]);
/* second axis inverted */
scaling[1] = fabsf(1.16f / -avg_excited[1]);
scaling[2] = fabsf(1.08f / avg_excited[2]);
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
goto out;
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
goto out;
}
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
goto out;
}
ret = OK;
out:
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
ret = ERROR;
}
} else {
warnx("mag scale calibration failed.");
}
return ret;
}
int HMC5883::calibrate(struct file *filp, unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret = 1;
uint8_t good_count = 0;
// XXX do something smarter here
int fd = (int)enable;
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
float sum_excited[3] = {0.0f, 0.0f, 0.0f};
/* expected axis scaling. The datasheet says that 766 will
* be places in the X and Y axes and 713 in the Z
* axis. Experiments show that in fact 766 is placed in X,
* and 713 in Y and Z. This is relative to a base of 660
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
warnx("starting mag scale calibration");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("failed to set 2Hz poll rate");
ret = 1;
goto out;
}
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("failed to enable sensor calibration mode");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to get scale / offsets for mag");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("WARNING: failed to set null scale / offsets for mag");
ret = 1;
goto out;
}
// discard 10 samples to let the sensor settle
for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
}
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("timed out waiting for sensor data");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("periodic read failed");
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
good_count++;
sum_excited[0] += cal[0];
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
//warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
warn("failed calibration");
ret = -EIO;
goto out;
}
#if 0
warnx("measurement avg: %.6f %.6f %.6f",
(double)sum_excited[0]/good_count,
(double)sum_excited[1]/good_count,
(double)sum_excited[2]/good_count);
#endif
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
ret = OK;
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("WARNING: failed to set new scale / offsets for mag");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("failed to set 1.1 Ga range");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("failed to disable sensor calibration mode");
}
if (ret == OK) {
if (!check_scale()) {
warnx("mag scale calibration successfully finished.");
} else {
warnx("mag scale calibration finished with invalid results.");
ret = ERROR;
}
} else {
warnx("mag scale calibration failed.");
}
return ret;
}
int IST8310::calibrate(struct file *filp, unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret = 1;
float total_x = 0.0f;
float total_y = 0.0f;
float total_z = 0.0f;
// XXX do something smarter here
int fd = (int)enable;
struct mag_calibration_s mscale_previous;
struct mag_calibration_s mscale_null;
mscale_null.x_offset = 0.0f;
mscale_null.x_scale = 1.0f;
mscale_null.y_offset = 0.0f;
mscale_null.y_scale = 1.0f;
mscale_null.z_offset = 0.0f;
mscale_null.z_scale = 1.0f;
float sum_in_test[3] = {0.0f, 0.0f, 0.0f};
float sum_in_normal[3] = {0.0f, 0.0f, 0.0f};
float *sum = &sum_in_normal[0];
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
PX4_WARN("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
PX4_WARN("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
PX4_WARN("FAILED: SENSORIOCSPOLLRATE 50Hz");
ret = 1;
goto out;
}
// discard 10 samples to let the sensor settle
/* read the sensor 50 times */
for (uint8_t p = 0; p < 2; p++) {
if (p == 1) {
/* start the Self test */
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
PX4_WARN("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
sum = &sum_in_test[0];
}
for (uint8_t i = 0; i < 30; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
PX4_WARN("ERROR: TIMEOUT 2");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
PX4_WARN("ERROR: READ 2");
ret = -EIO;
goto out;
}
if (i > 10) {
sum[0] += report.x_raw;
sum[1] += report.y_raw;
sum[2] += report.z_raw;
}
}
}
total_x = fabsf(sum_in_test[0] - sum_in_normal[0]);
total_y = fabsf(sum_in_test[1] - sum_in_normal[1]);
total_z = fabsf(sum_in_test[2] - sum_in_normal[2]);
ret = ((total_x + total_y + total_z) < (float)0.000001);
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
PX4_WARN("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
PX4_WARN("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (check_scale()) {
/* failed */
PX4_WARN("FAILED: SCALE");
ret = PX4_ERROR;
}
} else {
PX4_ERR("FAILED: CALIBRATION SCALE %d, %d, %d", (int)total_x, (int)total_y, (int)total_z);
}
return ret;
}
IC bool check_scale(const Fquaternion &q)
{
Fmatrix m;
m.rotation(q);
return check_scale(m);
}
// calculate single bone with key blending and callbck calling
void CKinematicsAnimated::CLBone(const CBoneData* bd,CBoneInstance& BONE_INST,const Fmatrix *parent,const CBlendInstance::BlendSVec &Blend, u8 channel_mask /*= (1<<0)*/)
{
u16 SelfID = bd->GetSelfID();
if (LL_GetBoneVisible(SelfID)){
if (BONE_INST.Callback_overwrite){
if (BONE_INST.Callback) BONE_INST.Callback(&BONE_INST);
} else {
CKey R[MAX_CHANNELS][MAX_BLENDED]; //all keys
CKey BK[MAX_CHANNELS][MAX_BLENDED]; //base keys
float BA[MAX_CHANNELS][MAX_BLENDED]; //all factors
int b_counts[MAX_CHANNELS] = {0,0,0,0}; //channel counts
//float BCA[MAX_CHANNELS] = {0,0,0,0}; //channel factors
BlendSVecCIt BI;
for (BI=Blend.begin(); BI!=Blend.end(); BI++)
{
CBlend* B = *BI;
int &b_count = b_counts[B->channel];
CKey* D = &R[B->channel][b_count];
if(!(channel_mask&(1<<B->channel)))
continue;
u8 channel = B->channel;
BA[channel][b_count] = B->blendAmount;
//BCA[channel] += B->blendAmount;
CMotion &M =*LL_GetMotion(B->motionID,SelfID);
Dequantize(*D,*B,M);
QR2Quat( M._keysR[0], BK[channel][b_count].Q );
if(M.test_flag(flTKeyPresent))
QT2T(M._keysT[0] ,M ,BK[channel][b_count].T );
else
BK[channel][b_count].T.set(M._initT);
++b_count;
/// PSGP.blerp (D,&K1,&K2,delta);
}
// Blend them together
CKey channels[MAX_CHANNELS];
float BC [MAX_CHANNELS];
u16 ch_count = 0;
for(u16 j= 0;MAX_CHANNELS>j;++j)
{
if(j!=0&&b_counts[j]==0)
continue;
//data for channel mix cycle based on ch_count
CKey &C = channels[ch_count];
BC[ch_count] = channel_factors[j];//3.f;//BCA[j]*
if(j != 0)
keys_substruct(R[j],BK[j],b_counts[j]);
MixInterlerp( C, R[j], BA[j], b_counts[j] );
++ch_count;
}
CKey Result;
//Mix channels
//MixInterlerp(Result,channels,BCA,ch_count);
MixChannels( Result, channels, BC, ch_count );
Fmatrix RES;
RES.mk_xform (Result.Q,Result.T);
BONE_INST.mTransform.mul_43(*parent,RES);
#ifdef DEBUG
if(!check_scale(RES))
{
VERIFY(check_scale(BONE_INST.mTransform));
}
/*
if(!is_similar(BONE_INST.mPrevTransform,RES,0.3f))
{
Msg("bone %s",*bd->name) ;
}
BONE_INST.mPrevTransform.set(RES);
*/
#endif
/*
if(BONE_INST.mTransform.c.y>10000)
{
Log("BLEND_INST",BLEND_INST.Blend.size());
Log("Bone",LL_BoneName_dbg(SelfID));
Msg("Result.Q %f,%f,%f,%f",Result.Q.x,Result.Q.y,Result.Q.z,Result.Q.w);
Log("Result.T",Result.T);
Log("lp parent",(u32)parent);
Log("parent",*parent);
Log("RES",RES);
Log("mT",BONE_INST.mTransform);
CBlend* B = *BI;
CMotion& M = *LL_GetMotion(B->motionID,SelfID);
float time = B->timeCurrent*float(SAMPLE_FPS);
u32 frame = iFloor(time);
u32 count = M.get_count();
float delta = time-float(frame);
Log("flTKeyPresent",M.test_flag(flTKeyPresent));
Log("M._initT",M._initT);
Log("M._sizeT",M._sizeT);
// translate
if (M.test_flag(flTKeyPresent))
{
CKeyQT* K1t = &M._keysT[(frame+0)%count];
CKeyQT* K2t = &M._keysT[(frame+1)%count];
Fvector T1,T2,Dt;
T1.x = float(K1t->x)*M._sizeT.x+M._initT.x;
T1.y = float(K1t->y)*M._sizeT.y+M._initT.y;
T1.z = float(K1t->z)*M._sizeT.z+M._initT.z;
T2.x = float(K2t->x)*M._sizeT.x+M._initT.x;
T2.y = float(K2t->y)*M._sizeT.y+M._initT.y;
T2.z = float(K2t->z)*M._sizeT.z+M._initT.z;
Dt.lerp (T1,T2,delta);
Msg("K1t %d,%d,%d",K1t->x,K1t->y,K1t->z);
Msg("K2t %d,%d,%d",K2t->x,K2t->y,K2t->z);
Log("count",count);
Log("frame",frame);
Log("T1",T1);
Log("T2",T2);
Log("delta",delta);
Log("Dt",Dt);
}else
{
D->T.set (M._initT);
}
VERIFY(0);
}
*/
if (BONE_INST.Callback) BONE_INST.Callback(&BONE_INST);
}
BONE_INST.mRenderTransform.mul_43(BONE_INST.mTransform,bd->m2b_transform);
}
}
// Make sure all spin buttons everywhere always have correct values
void validate_value_change (GtkAdjustment *adj_changed, gpointer user_data)
{
print_properties_D *dialog = (print_properties_D *)gtk_object_get_data (GTK_OBJECT (user_data), "dialog") ;
check_scale (dialog, adj_changed) ;
}
int HMC5883::calibrate(struct file *filp, unsigned enable)
{
struct mag_report report;
ssize_t sz;
int ret = 1;
uint8_t good_count = 0;
// XXX do something smarter here
int fd = (int)enable;
struct mag_scale mscale_previous = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
struct mag_scale mscale_null = {
0.0f,
1.0f,
0.0f,
1.0f,
0.0f,
1.0f,
};
float sum_excited[3] = {0.0f, 0.0f, 0.0f};
/* expected axis scaling. The datasheet says that 766 will
* be places in the X and Y axes and 713 in the Z
* axis. Experiments show that in fact 766 is placed in X,
* and 713 in Y and Z. This is relative to a base of 660
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
// discard 10 samples to let the sensor settle
for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 1");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
}
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
fds.fd = fd;
fds.events = POLLIN;
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
warn("ERROR: TIMEOUT 2");
goto out;
}
/* now go get it */
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
float cal[3] = {fabsf(expected_cal[0] / report.x),
fabsf(expected_cal[1] / report.y),
fabsf(expected_cal[2] / report.z)};
if (cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
good_count++;
sum_excited[0] += cal[0];
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
}
if (good_count < 5) {
ret = -EIO;
goto out;
}
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
ret = OK;
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
if (check_scale()) {
/* failed */
warnx("FAILED: SCALE");
ret = ERROR;
}
}
return ret;
}
void go_up(t_struct *st)
{
if (check_door(st) == -1)
if (check_up(st) == -1)
check_scale(st);
}
int quadbuilder_create(quadbuilder_t* qb) {
int iA=0, iB, iC, iD, newpoint;
int ninbox;
int i, j;
int iAalloc;
quad q;
pquad_t* qb_pquads;
// ensure the arrays are large enough...
if (qb->Nstars > qb->Ncq) {
// (free and malloc rather than realloc because we don't care about
// the previous contents)
free(qb->inbox);
free(qb->pquads);
qb->Ncq = qb->Nstars;
qb->inbox = calloc(qb->Nstars, sizeof(int));
qb->pquads = calloc(qb->Nstars * qb->Nstars, sizeof(pquad_t));
if (!qb->inbox || !qb->pquads) {
ERROR("quad-builder: failed to malloc qb->inbox or qb->pquads. Nstars=%i.\n", qb->Nstars);
return -1;
}
}
qb_pquads = qb->pquads;
/*
Each time through the "for" loop below, we consider a new
star ("newpoint"). First, we try building all quads that
have the new star on the diagonal (star B). Then, we try
building all quads that have the star not on the diagonal
(star D).
Note that we keep the invariants iA < iB and iC < iD.
*/
memset(&q, 0, sizeof(quad));
for (newpoint=0; newpoint<qb->Nstars; newpoint++) {
pquad_t* pq;
// quads with the new star on the diagonal:
iB = newpoint;
for (iA = 0; iA < newpoint; iA++) {
pq = qb_pquads + iA*qb->Nstars + iB;
pq->inbox = NULL;
pq->ninbox = 0;
pq->iA = iA;
pq->iB = iB;
check_scale(qb, pq);
if (!pq->scale_ok)
continue;
q.star[0] = pq->staridA;
q.star[1] = pq->staridB;
pq->check_ok = TRUE;
if (qb->check_AB_stars)
pq->check_ok = qb->check_AB_stars(qb, pq, qb->check_AB_stars_token);
if (!pq->check_ok)
continue;
// list the possible internal stars...
ninbox = 0;
for (iC = 0; iC < newpoint; iC++) {
if ((iC == iA) || (iC == iB))
continue;
qb->inbox[ninbox] = iC;
ninbox++;
}
// check which ones are inside the box...
ninbox = check_inbox(pq, qb->inbox, ninbox, qb->starxyz);
//if (!ninbox)
//continue;
if (ninbox && qb->check_internal_stars)
ninbox = qb->check_internal_stars(qb, q.star[0], q.star[1], qb->inbox, ninbox, qb->check_internal_stars_token);
//if (!ninbox)
//continue;
add_interior_stars(qb, ninbox, qb->inbox, &q, 2, qb->dimquads, 0);
if (qb->stop_creating)
goto theend;
pq->inbox = malloc(qb->Nstars * sizeof(int));
if (!pq->inbox) {
ERROR("hpquads: failed to malloc pq->inbox.\n");
exit(-1);
}
pq->ninbox = ninbox;
memcpy(pq->inbox, qb->inbox, ninbox * sizeof(int));
debug("iA=%i, iB=%i: saved %i 'inbox' entries.\n", iA, iB, ninbox);
}
iAalloc = iA;
// quads with the new star not on the diagonal:
iD = newpoint;
for (iA = 0; iA < newpoint; iA++) {
for (iB = iA + 1; iB < newpoint; iB++) {
pq = qb_pquads + iA*qb->Nstars + iB;
if (!(pq->scale_ok && pq->check_ok))
continue;
// check if this new star is in the box.
qb->inbox[0] = iD;
ninbox = check_inbox(pq, qb->inbox, 1, qb->starxyz);
if (!ninbox)
continue;
if (qb->check_internal_stars)
ninbox = qb->check_internal_stars(qb, q.star[0], q.star[1], qb->inbox, ninbox, qb->check_internal_stars_token);
if (!ninbox)
continue;
pq->inbox[pq->ninbox] = iD;
pq->ninbox++;
q.star[0] = pq->staridA;
q.star[1] = pq->staridB;
add_interior_stars(qb, pq->ninbox, pq->inbox, &q, 2, qb->dimquads, 0);
if (qb->stop_creating) {
iA = iAalloc;
goto theend;
}
}
}
}
theend:
for (i=0; i<imin(qb->Nstars, newpoint+1); i++) {
int lim = (i == newpoint) ? iA : i;
for (j=0; j<lim; j++) {
pquad_t* pq = qb_pquads + j*qb->Nstars + i;
free(pq->inbox);
pq->inbox = NULL;
}
}
return 0;
}
