static int oid_measure_resistor(int pin0, int pin1)
{
int mohm = -1;
dmm_config("R");
matrix_reset();
matrix_connect(RBUS_IOP, oid_pinmap[pin0]);
matrix_connect(RBUS_VIP, oid_pinmap[pin0]);
matrix_connect(RBUS_ION, oid_pinmap[pin1]);
matrix_connect(RBUS_VIN, oid_pinmap[pin1]);
if(!matrix_execute()) {
oid_show_progress(PROGRESS_WAIT);
#if 1
time_t deadline = time_get(600);
while(time_left(deadline) > 0) {
instr_update();
}
#else
sys_mdelay(300);
#endif
mohm = dmm_read();
mohm += oid_mohm_offset;
mohm = (mohm < 0) ? 0 : mohm;
}
return mohm;
}
/*************************************************
Direction Cosine Matrix IMU: Theory
William Premerlani and Paul Bizard
Numerical errors will gradually reduce the orthogonality conditions expressed by equation 5
to approximations rather than identities. In effect, the axes in the two frames of reference no
longer describe a rigid body. Fortunately, numerical error accumulates very slowly, so it is a
simple matter to stay ahead of it.
We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ.
*/
void
AP_DCM::normalize(void)
{
float error = 0;
Vector3f temporary[3];
int problem = 0;
error = _dcm_matrix.a * _dcm_matrix.b; // eq.18
temporary[0] = _dcm_matrix.b;
temporary[1] = _dcm_matrix.a;
temporary[0] = _dcm_matrix.a - (temporary[0] * (0.5f * error)); // eq.19
temporary[1] = _dcm_matrix.b - (temporary[1] * (0.5f * error)); // eq.19
temporary[2] = temporary[0] % temporary[1]; // c= a x b // eq.20
_dcm_matrix.a = renorm(temporary[0], problem);
_dcm_matrix.b = renorm(temporary[1], problem);
_dcm_matrix.c = renorm(temporary[2], problem);
if (problem == 1) { // Our solution is blowing up and we will force back to initial condition. Hope we are not upside down!
matrix_reset();
}
}
static int oid_measure_voltage(int pin0, int pin1)
{
int mv = -1;
dmm_config("V");
matrix_reset();
matrix_connect(RBUS_VIP, oid_pinmap[pin0]);
matrix_connect(RBUS_VIN, oid_pinmap[pin1]);
if(!matrix_execute()) {
oid_show_progress(PROGRESS_WAIT);
#if 1
time_t deadline = time_get(600);
while(time_left(deadline) > 0) {
instr_update();
}
#else
sys_mdelay(300);
#endif
mv = dmm_read();
mv = (mv < 0) ? (-mv) : mv;
}
return mv;
}
