part3
///////////////////////////////////////////
void fReadMPU9250 ( uint8_t byteReadSize, uint8_t addressToRead )
{
esp_err_t intError;
spi_transaction_t trans_desc;
trans_desc = { };
trans_desc.addr = 0;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = (8 * 2) + (8 * byteReadSize) ; // total data bits
trans_desc.tx_buffer = txData;
trans_desc.rxlength = byteReadSize * 8 ; // Number of bits NOT number of bytes
trans_desc.rx_buffer = rxData;
txData[0] = addressToRead | SPI_READ;
txData[1] = 0x0;
intError = spi_device_transmit( hAG, &trans_desc);
if ( intError != 0 )
{
log_i ( " WHO I am MPU9250. Transmitting error = %d", intError );
}
} // void fReadMPU9250 ( uint8_t byteReadSize, uint8_t addressToRead )
void fWriteSPIdata8bits( uint8_t address, uint8_t DataToSend)
{
esp_err_t intError;
spi_transaction_t trans_desc;
trans_desc = { };
trans_desc.addr = 0;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = 8 * 2; // total data bits
trans_desc.tx_buffer = txData;
txData[0] = address;
txData[1] = DataToSend;
intError = spi_device_transmit( hAG, &trans_desc);
if ( intError != 0 )
{
log_i( " Transmitting error = %d", intError );
}
} // void fSendSPI( uint8_t count, uint8_t address, uint8_t DataToSend)
///////////////////////////////////////////////////////////////////////
void fReadAK8963( uint8_t subAddress, uint8_t count )
{
// set slave 0 to the AK8963 and set for read
fWriteSPIdata8bits ( I2C_SLV0_ADDR, AK8963_I2C_ADDR | I2C_READ_FLAG );
// set the register to the desired AK8963 sub address
// I2C_SLV0_REG, subAddress
fWriteSPIdata8bits ( I2C_SLV0_REG, subAddress );
// enable I2C and request the bytes
// I2C_SLV0_CTRL, I2C_SLV0_EN | count
fWriteSPIdata8bits ( I2C_SLV0_CTRL, I2C_SLV0_EN | count );
vTaskDelay ( 1 );
// read result in rxData EXT_SENS_DATA_00,count,dest
// EXT_SENS_DATA_00,count,dest
} // fReadAK8963(uint8_t subAddress, uint8_t count, uint8_t* dest)
////////////////////////////////////////////////////////////////////////////////////////////
void fWrite_AK8963 ( uint8_t subAddress, uint8_t dataAK8963 )
{
fWriteSPIdata8bits ( I2C_SLV0_ADDR, AK8963_I2C_ADDR );
fWriteSPIdata8bits ( I2C_SLV0_REG, subAddress );
fWriteSPIdata8bits ( I2C_SLV0_DO, dataAK8963 );
fWriteSPIdata8bits ( I2C_SLV0_CTRL, I2C_SLV0_EN | (uint8_t)1) ;
//
vTaskDelay ( 1 );
} // void fWrite_AK8963 ( uint8_t subAddress, uint8_t dataAK8963 )
/////////////////////////////////////////////////////////////////////////////////////////////
void fwriteMUP9250register ( uint8_t addr, uint8_t sendData )
{
esp_err_t intError;
spi_transaction_t trans_desc = {};
trans_desc.addr = addr;
trans_desc.cmd = 0;
trans_desc.flags = 0;
trans_desc.length = 8 * 1 ; // total data bits
trans_desc.tx_buffer = txData;
txData[0] = sendData;
intError = spi_device_transmit( hAG, &trans_desc);
if ( intError != 0 )
{
log_i( " Transmitting error = %d", intError );
}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
} // void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
//////////////////////////////////////////////
