void MMA8451::pickle(float *result, uint32_t still_msk) {
(void)still_msk;
int32_t raw[3];
raw[0] = complement2signed(rxbuf[1], rxbuf[2]);
raw[1] = complement2signed(rxbuf[3], rxbuf[4]);
raw[2] = complement2signed(rxbuf[5], rxbuf[6]);
/* convert to NUE coordinate system */
sort3(raw, *sortmtrx);
raw[0] *= *xpol;
raw[1] *= *ypol;
raw[2] *= *zpol;
mavlink_out_raw_imu_struct.xacc = raw[0];
mavlink_out_raw_imu_struct.yacc = raw[1];
mavlink_out_raw_imu_struct.zacc = raw[2];
comp_data.acc_i16[0] = (1000 * (raw[0] + *xoffset)) / *xsens;
comp_data.acc_i16[1] = (1000 * (raw[1] + *yoffset)) / *ysens;
comp_data.acc_i16[2] = (1000 * (raw[2] + *zoffset)) / *zsens;
result[0] = comp_data.acc_i16[0] / 1000.0f;
result[1] = comp_data.acc_i16[1] / 1000.0f;
result[2] = comp_data.acc_i16[2] / 1000.0f;
mavlink_out_scaled_imu_struct.xacc = comp_data.acc_i16[0];
mavlink_out_scaled_imu_struct.yacc = comp_data.acc_i16[1];
mavlink_out_scaled_imu_struct.zacc = comp_data.acc_i16[2];
//mavlink_out_scaled_imu_struct.zacc = vector3d_modulus(result) * 1000;
}
static msg_t PollAccelThread(void *semp){
chRegSetThreadName("PollAccel");
msg_t sem_status = RDY_OK;
struct EventListener self_el;
chEvtRegister(&init_event, &self_el, INIT_FAKE_EVID);
while (TRUE) {
sem_status = chBSemWaitTimeout((BinarySemaphore*)semp, MS2ST(20));
txbuf[0] = ACCEL_STATUS;
if ((i2c_transmit(mma8451addr, txbuf, 1, rxbuf, 7) == RDY_OK) && (sem_status == RDY_OK)){
raw_data.xacc = complement2signed(rxbuf[1], rxbuf[2]);
raw_data.yacc = complement2signed(rxbuf[3], rxbuf[4]);
raw_data.zacc = complement2signed(rxbuf[5], rxbuf[6]);
/* there is no need of correcting of placement. Just get milli g */
mavlink_raw_imu_struct.xacc = raw_data.xacc * *xpol;
mavlink_raw_imu_struct.yacc = raw_data.yacc * *ypol;
mavlink_raw_imu_struct.zacc = raw_data.zacc * *zpol;
comp_data.xacc = 1000 * (((int32_t)raw_data.xacc) * *xpol + *xoffset) / *xsens;
comp_data.yacc = 1000 * (((int32_t)raw_data.yacc) * *ypol + *yoffset) / *ysens;
comp_data.zacc = 1000 * (((int32_t)raw_data.zacc) * *zpol + *zoffset) / *zsens;
/* fill scaled debug struct */
mavlink_scaled_imu_struct.xacc = comp_data.xacc;
mavlink_scaled_imu_struct.yacc = comp_data.yacc;
mavlink_scaled_imu_struct.zacc = comp_data.zacc;
}
else{
raw_data.xacc = -32768;
raw_data.yacc = -32768;
raw_data.zacc = -32768;
mavlink_raw_imu_struct.xacc = -32768;
mavlink_raw_imu_struct.yacc = -32768;
mavlink_raw_imu_struct.zacc = -32768;
mavlink_scaled_imu_struct.xacc = -32768;
mavlink_scaled_imu_struct.yacc = -32768;
mavlink_scaled_imu_struct.zacc = -32768;
}
if (chThdSelf()->p_epending & EVENT_MASK(SIGHALT_EVID))
chThdExit(RDY_OK);
}
return 0;
}
int magGetRegValues() {
status = RDY_OK;
systime_t tmo = MS2ST(4);
static uint8_t rxbuf[6];
static uint8_t txbuf[6];
txbuf[0] = 3;
i2cAcquireBus(driver);
status =
i2cMasterTransmitTimeout(driver, mag_addres, txbuf, 1, rxbuf, 6, tmo);
i2cReleaseBus(driver);
if (status != RDY_OK) {
}
X_m = complement2signed(rxbuf[0], rxbuf[1]);
Y_m = complement2signed(rxbuf[4], rxbuf[5]);
Z_m = complement2signed(rxbuf[2], rxbuf[3]);
return 0;
}
/*
* This function reads data from MPU60X0. Input is register address and lenght of buffer to be read.
*/
void mpu_i2c_read_data(uint8_t addr, uint8_t length){
uint8_t i = 0;
uint8_t txbuf[1];
uint8_t rxbuf[length];
int16_t gyro[3];
int16_t acc[3];
txbuf[0] = addr;
for(i=0;i<length;i++)rxbuf[i] = 0x00;
i2c_transmit(MPU_ADDR, txbuf, 1, rxbuf, length);
acc[0] = complement2signed(rxbuf[0], rxbuf[1]);
acc[1] = complement2signed(rxbuf[2], rxbuf[3]);
acc[2] = complement2signed(rxbuf[4], rxbuf[5]);
// rxbuf[6] and rxbuf[7] is temperature(ignored at this time)
gyro[0] = complement2signed(rxbuf[8], rxbuf[9]);
gyro[1] = complement2signed(rxbuf[10], rxbuf[11]);
gyro[2] = complement2signed(rxbuf[12], rxbuf[13]);
imu_data.acc_x = acc[0] / 4096.f;
imu_data.acc_y = acc[1] / 4096.f;
imu_data.acc_z = acc[2] / 4096.f;
imu_data.gyro_x = gyro[0] / 16.4f;
imu_data.gyro_y = gyro[1] / 16.4f;
imu_data.gyro_z = gyro[2] / 16.4f;
}
/*
* Application entry point.
*/
int main(void) {
msg_t status = RDY_OK;
systime_t tmo = MS2ST(4);
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Prepares the Serial driver 2
*/
sdStart(&SD2, NULL); /* Default is 38400-8-N-1.*/
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/* I2C interface #2 */
static const I2CConfig i2cfg2 = {
OPMODE_I2C,
400000,
FAST_DUTY_CYCLE_16_9,
};
i2cStart(&I2CD2, &i2cfg2);
/**
* Prepares the accelerometer
*/
txbuf[0] = ACCEL_CTRL_REG1; /* register address */
txbuf[1] = 0x1;
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, mma8451_addr, txbuf, 2, rxbuf, 0, tmo);
i2cReleaseBus(&I2CD2);
if (status != RDY_OK){
errors = i2cGetErrors(&I2CD2);
}
/*
* Normal main() thread activity, nothing in this test.
*/
while (TRUE) {
palTogglePad(GPIOB, GPIOB_LED_B);
chThdSleepMilliseconds(100);
txbuf[0] = ACCEL_OUT_DATA; /* register address */
i2cAcquireBus(&I2CD2);
status = i2cMasterTransmitTimeout(&I2CD2, mma8451_addr, txbuf, 1, rxbuf, 6, tmo);
i2cReleaseBus(&I2CD2);
if (status != RDY_OK){
errors = i2cGetErrors(&I2CD2);
}
acceleration_x = complement2signed(rxbuf[0], rxbuf[1]);
acceleration_y = complement2signed(rxbuf[2], rxbuf[3]);
acceleration_z = complement2signed(rxbuf[4], rxbuf[5]);
print("x: ");
printn(acceleration_x);
print(" y: ");
printn(acceleration_y);
print(" z: ");
printn(acceleration_z);
println("");
}
}
void TMP75::pickle(void){
raw_data.temp_tmp75 = complement2signed(rxbuf[0], rxbuf[1]);
comp_data.temp_onboard = (int16_t)((100 * (int32_t)raw_data.temp_tmp75) / 256);
mavlink_out_scaled_pressure_struct.temperature = comp_data.temp_onboard;
}
