mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-19 19:39:05 +08:00
ist8310: remove commented out and dead code
This code just makes it harder to grok the driver. When support for this feature is available we can bring it back.
This commit is contained in:
Lucas De Marchi
Lorenz Meier
parent
ade6336fd0
commit
e40209ead9
@ -84,18 +84,8 @@
|
||||
* The datasheet gives 200Hz maximum measurement rate, but it's not true according to tech support from iSentek*/
|
||||
#define IST8310_CONVERSION_INTERVAL (1000000 / 100) /* microseconds */
|
||||
|
||||
#define IST8310_BUS_I2C_ADDR 0xE
|
||||
#define IST8310_DEFAULT_BUS_SPEED 400000
|
||||
|
||||
/* compensation matrix scalar */
|
||||
#define IST8310_COMPENSATION_MATRIX_M 3/20.0f
|
||||
|
||||
/* Cross-axis calibration sensitivity and bit shift setting */
|
||||
#define OTPsensitivity (330)
|
||||
#define CROSSAXISINV_BITSHIFT (16)
|
||||
|
||||
/* Use float calculation */
|
||||
#define IST_CROSS_AXIS_CALI_FLOAT
|
||||
#define IST8310_BUS_I2C_ADDR 0xE
|
||||
#define IST8310_DEFAULT_BUS_SPEED 400000
|
||||
|
||||
/* Hardware definitions */
|
||||
|
||||
@ -233,15 +223,6 @@ private:
|
||||
uint8_t _ctl3_reg;
|
||||
uint8_t _ctl4_reg;
|
||||
|
||||
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
float crossaxis_inv[9];
|
||||
#else
|
||||
int64_t crossaxis_inv[9];
|
||||
#endif
|
||||
|
||||
int32_t crossaxis_det[1];
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
@ -389,22 +370,6 @@ private:
|
||||
*/
|
||||
int set_selftest(unsigned enable);
|
||||
|
||||
/**
|
||||
* Initiate cross-axis matrix
|
||||
*
|
||||
* @param bitshift The bit shift configuration for different chip, used when using integer calculation
|
||||
* @param enable Enable cross-axis compensation
|
||||
* @return Initialization succeed
|
||||
*/
|
||||
bool crossaxis_matrix_init(int bitshift, int enable);
|
||||
|
||||
/**
|
||||
* Transfer the raw sensor data using cross-axis matrix
|
||||
*
|
||||
* @param xyz The pointer of the sensor buffer
|
||||
*/
|
||||
void crossaxis_transformation(int16_t *xyz);
|
||||
|
||||
/* this class has pointer data members, do not allow copying it */
|
||||
IST8310(const IST8310 &);
|
||||
IST8310 operator=(const IST8310 &);
|
||||
@ -813,27 +778,6 @@ IST8310::reset()
|
||||
_ctl4_reg = CTRL4_SRPD;
|
||||
write_reg(ADDR_CTRL4, _ctl4_reg);
|
||||
|
||||
/* initiate cross-axis matrix */
|
||||
// uint8_t wbuffer1[2] = {0};
|
||||
// uint8_t wbuffer2[2] = {0};
|
||||
// uint8_t try_times = 0;
|
||||
// while (++try_times <= 10) {
|
||||
// if (read(ADDR_Y11_Low, wbuffer1, 2) == OK) {
|
||||
// if (read(ADDR_Y11_Low, wbuffer2, 2) == OK) {
|
||||
// if ((wbuffer1[0] == wbuffer2[0]) && (wbuffer1[1] == wbuffer2[1])) {
|
||||
// int crossaxis_enable = 0;
|
||||
// uint8_t cross_mask = 0xFF;
|
||||
// if ((wbuffer1[0] == cross_mask) && (wbuffer1[1] == cross_mask)) crossaxis_enable = 0;
|
||||
// else crossaxis_enable = 1;
|
||||
//
|
||||
// if (crossaxis_matrix_init(CROSSAXISINV_BITSHIFT, crossaxis_enable)) {
|
||||
// return OK;
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@ -985,9 +929,6 @@ IST8310::collect()
|
||||
report.y = (((int16_t)report_buffer.y[1]) << 8) | (int16_t)report_buffer.y[0];
|
||||
report.z = (((int16_t)report_buffer.z[1]) << 8) | (int16_t)report_buffer.z[0];
|
||||
|
||||
/* perform cross-axis compensation */
|
||||
// crossaxis_transformation((int16_t *)&report);
|
||||
|
||||
/* temperature measurement is not available on IST8310 */
|
||||
new_report.temperature = 0;
|
||||
|
||||
@ -1249,164 +1190,6 @@ IST8310::set_selftest(unsigned enable)
|
||||
return !(str == str_reg_ret);
|
||||
}
|
||||
|
||||
bool
|
||||
IST8310::crossaxis_matrix_init(int bitshift, int enable)
|
||||
{
|
||||
int i = 0;
|
||||
uint8_t crossxbuf[6];
|
||||
uint8_t crossybuf[6];
|
||||
uint8_t crosszbuf[6];
|
||||
uint8_t crossxbuf2[6];
|
||||
uint8_t crossybuf2[6];
|
||||
uint8_t crosszbuf2[6];
|
||||
int16_t OTPcrossaxis[9] = {0};
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
float inv[9] = {0};
|
||||
#else
|
||||
int64_t inv[9] = {0};
|
||||
#endif
|
||||
|
||||
if (enable == 0) {
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
*crossaxis_inv = 1;
|
||||
#else
|
||||
*crossaxis_inv = (1 << bitshift);
|
||||
#endif
|
||||
*(crossaxis_inv + 1) = 0;
|
||||
*(crossaxis_inv + 2) = 0;
|
||||
*(crossaxis_inv + 3) = 0;
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
*(crossaxis_inv + 4) = 1;
|
||||
#else
|
||||
*(crossaxis_inv + 4) = (1 << bitshift);
|
||||
#endif
|
||||
*(crossaxis_inv + 5) = 0;
|
||||
*(crossaxis_inv + 6) = 0;
|
||||
*(crossaxis_inv + 7) = 0;
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
*(crossaxis_inv + 8) = 1;
|
||||
#else
|
||||
*(crossaxis_inv + 8) = (1 << bitshift);
|
||||
#endif
|
||||
*crossaxis_det = 1;
|
||||
|
||||
return true;
|
||||
|
||||
} else {
|
||||
bool pass_all = false;
|
||||
uint8_t retry_times = 0;
|
||||
|
||||
do {
|
||||
pass_all = true;
|
||||
|
||||
if (read(ADDR_Y11_Low, crossxbuf, 6) && read(ADDR_Y21_Low, crossybuf, 6) && read(ADDR_Y31_Low, crosszbuf, 6)) {
|
||||
if (read(ADDR_Y11_Low, crossxbuf2, 6) && read(ADDR_Y21_Low, crossybuf2, 6) && read(ADDR_Y31_Low, crosszbuf2, 6)) {
|
||||
for (i = 0; i < 6; ++i) {
|
||||
if (crossxbuf[i] != crossxbuf2[i]) {
|
||||
pass_all = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (pass_all) { break; }
|
||||
} while (++retry_times < 5);
|
||||
|
||||
if (retry_times >= 5 && !pass_all) { return false; }
|
||||
|
||||
OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | ((int16_t) crossxbuf[0]);
|
||||
OTPcrossaxis[3] = ((int16_t) crossxbuf[3]) << 8 | ((int16_t) crossxbuf[2]);
|
||||
OTPcrossaxis[6] = ((int16_t) crossxbuf[5]) << 8 | ((int16_t) crossxbuf[4]);
|
||||
OTPcrossaxis[1] = ((int16_t) crossybuf[1]) << 8 | ((int16_t) crossybuf[0]);
|
||||
OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | ((int16_t) crossybuf[2]);
|
||||
OTPcrossaxis[7] = ((int16_t) crossybuf[5]) << 8 | ((int16_t) crossybuf[4]);
|
||||
OTPcrossaxis[2] = ((int16_t) crosszbuf[1]) << 8 | ((int16_t) crosszbuf[0]);
|
||||
OTPcrossaxis[5] = ((int16_t) crosszbuf[3]) << 8 | ((int16_t) crosszbuf[2]);
|
||||
OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | ((int16_t) crosszbuf[4]);
|
||||
*crossaxis_det = ((int32_t)OTPcrossaxis[0]) * OTPcrossaxis[4] * OTPcrossaxis[8] +
|
||||
((int32_t)OTPcrossaxis[1]) * OTPcrossaxis[5] * OTPcrossaxis[6] +
|
||||
((int32_t)OTPcrossaxis[2]) * OTPcrossaxis[3] * OTPcrossaxis[7] -
|
||||
((int32_t)OTPcrossaxis[0]) * OTPcrossaxis[5] * OTPcrossaxis[7] -
|
||||
((int32_t)OTPcrossaxis[2]) * OTPcrossaxis[4] * OTPcrossaxis[6] -
|
||||
((int32_t)OTPcrossaxis[1]) * OTPcrossaxis[3] * OTPcrossaxis[8];
|
||||
|
||||
if (*crossaxis_det == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
inv[0] = (float)OTPcrossaxis[4] * OTPcrossaxis[8] - (float)OTPcrossaxis[5] * OTPcrossaxis[7];
|
||||
inv[1] = (float)OTPcrossaxis[2] * OTPcrossaxis[7] - (float)OTPcrossaxis[1] * OTPcrossaxis[8];
|
||||
inv[2] = (float)OTPcrossaxis[1] * OTPcrossaxis[5] - (float)OTPcrossaxis[2] * OTPcrossaxis[4];
|
||||
inv[3] = (float)OTPcrossaxis[5] * OTPcrossaxis[6] - (float)OTPcrossaxis[3] * OTPcrossaxis[8];
|
||||
inv[4] = (float)OTPcrossaxis[0] * OTPcrossaxis[8] - (float)OTPcrossaxis[2] * OTPcrossaxis[6];
|
||||
inv[5] = (float)OTPcrossaxis[2] * OTPcrossaxis[3] - (float)OTPcrossaxis[0] * OTPcrossaxis[5];
|
||||
inv[6] = (float)OTPcrossaxis[3] * OTPcrossaxis[7] - (float)OTPcrossaxis[4] * OTPcrossaxis[6];
|
||||
inv[7] = (float)OTPcrossaxis[1] * OTPcrossaxis[6] - (float)OTPcrossaxis[0] * OTPcrossaxis[7];
|
||||
inv[8] = (float)OTPcrossaxis[0] * OTPcrossaxis[4] - (float)OTPcrossaxis[1] * OTPcrossaxis[3];
|
||||
|
||||
for (i = 0; i < 9; i++) {
|
||||
crossaxis_inv[i] = inv[i] * OTPsensitivity / (*crossaxis_det);
|
||||
}
|
||||
|
||||
#else
|
||||
inv[0] = (int64_t)OTPcrossaxis[4] * OTPcrossaxis[8] - (int64_t)OTPcrossaxis[5] * OTPcrossaxis[7];
|
||||
inv[1] = (int64_t)OTPcrossaxis[2] * OTPcrossaxis[7] - (int64_t)OTPcrossaxis[1] * OTPcrossaxis[8];
|
||||
inv[2] = (int64_t)OTPcrossaxis[1] * OTPcrossaxis[5] - (int64_t)OTPcrossaxis[2] * OTPcrossaxis[4];
|
||||
inv[3] = (int64_t)OTPcrossaxis[5] * OTPcrossaxis[6] - (int64_t)OTPcrossaxis[3] * OTPcrossaxis[8];
|
||||
inv[4] = (int64_t)OTPcrossaxis[0] * OTPcrossaxis[8] - (int64_t)OTPcrossaxis[2] * OTPcrossaxis[6];
|
||||
inv[5] = (int64_t)OTPcrossaxis[2] * OTPcrossaxis[3] - (int64_t)OTPcrossaxis[0] * OTPcrossaxis[5];
|
||||
inv[6] = (int64_t)OTPcrossaxis[3] * OTPcrossaxis[7] - (int64_t)OTPcrossaxis[4] * OTPcrossaxis[6];
|
||||
inv[7] = (int64_t)OTPcrossaxis[1] * OTPcrossaxis[6] - (int64_t)OTPcrossaxis[0] * OTPcrossaxis[7];
|
||||
inv[8] = (int64_t)OTPcrossaxis[0] * OTPcrossaxis[4] - (int64_t)OTPcrossaxis[1] * OTPcrossaxis[3];
|
||||
|
||||
for (i = 0; i < 9; i++) {
|
||||
crossaxis_inv[i] = (inv[i] << bitshift) * OTPsensitivity;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void
|
||||
IST8310::crossaxis_transformation(int16_t *xyz)
|
||||
{
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
float outputtmp[3];
|
||||
#else
|
||||
int64_t outputtmp[3];
|
||||
#endif
|
||||
|
||||
outputtmp[0] = xyz[0] * crossaxis_inv[0] +
|
||||
xyz[1] * crossaxis_inv[1] +
|
||||
xyz[2] * crossaxis_inv[2];
|
||||
|
||||
outputtmp[1] = xyz[0] * crossaxis_inv[3] +
|
||||
xyz[1] * crossaxis_inv[4] +
|
||||
xyz[2] * crossaxis_inv[5];
|
||||
|
||||
outputtmp[2] = xyz[0] * crossaxis_inv[6] +
|
||||
xyz[1] * crossaxis_inv[7] +
|
||||
xyz[2] * crossaxis_inv[8];
|
||||
#ifdef IST_CROSS_AXIS_CALI_FLOAT
|
||||
xyz[0] = (short)(outputtmp[0]);
|
||||
xyz[1] = (short)(outputtmp[1]);
|
||||
xyz[2] = (short)(outputtmp[2]);
|
||||
#else
|
||||
int i = 0;
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
outputtmp[i] = outputtmp[i] / (*crossaxis_det);
|
||||
}
|
||||
|
||||
xyz[0] = (short)(outputtmp[0] >> CROSSAXISINV_BITSHIFT);
|
||||
xyz[1] = (short)(outputtmp[1] >> CROSSAXISINV_BITSHIFT);
|
||||
xyz[2] = (short)(outputtmp[2] >> CROSSAXISINV_BITSHIFT);
|
||||
#endif
|
||||
}
|
||||
|
||||
int
|
||||
IST8310::write_reg(uint8_t reg, uint8_t val)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user