void SNShield::getAll(){
float acc[3];
this->temp = getTemp();
this->lux = getLux();
readAccelData(acc);
this->accx = acc[0];
this->accy = acc[1];
this->accz = acc[2];
}
bool MPU6050::getMotion6Counts(int16_t * ax, int16_t * ay, int16_t * az, int16_t * gx, int16_t * gy, int16_t * gz)
{
// If data ready bit set, all data registers have new data
if (readByte(INT_STATUS) & 0x01) { // check if data ready interrupt
readAccelData(ax, ay, az); // Read the x/y/z adc values
readGyroData(gx, gy, gz); // Read the x/y/z adc values
return true;
}
return false;
}
void loop()
{
char source;
if (digitalRead(int1Pin)==1) // Interrupt pin, should probably attach to interrupt function
{
int accelCount[3]; // Stores the 12-bit signed value
readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
float accelG[3]; // Stores the real accel value in g's
int i;
for(i=0;i<3;i++)
{
accelG[i] = (float) accelCount[i] / ((1<<12)/(2*GSCALE)); // get actual g value, this depends on scale being set
}
// Print out values
int j;
for(j=0;j<3;j++)
{
//printf(accelG[j], 4); // Print g values
printf("%G",accelG[j]);
printf("\t"); // tabs in between axes
}
printf("\n");
}
// If int2 goes high, either p/l has changed or there's been a single/double tap
if (digitalRead(int2Pin)==1)
{
source = readRegister(0x0C); // Read the interrupt source reg.
if ((source & 0x10)==0x10) // If the p/l bit is set, go check those registers
portraitLandscapeHandler();
else if ((source & 0x08)==0x08) // Otherwise, if tap register is set go check that
tapHandler();
}
//Attempt a delay
sleep(1);
}
/**
* @brief Function for application main entry.
*/
int main(void)
{
uint8_t data;
bool who_am_i;
bool init = true;
nrf_gpio_cfg_output (LED_0);
nrf_gpio_cfg_output (LED_1);
nrf_gpio_pin_set(LED_0);
simple_uart_config(RTS_PIN_NUMBER, TX_PIN_NUMBER, CTS_PIN_NUMBER, RX_PIN_NUMBER, HWFC);
simple_uart_putstring((const uint8_t *)" \r\nStart I2C \r\n");
nrf_delay_ms(500);
if(!twi_master_init()){
while (true)
{
nrf_gpio_pin_set(LED_0);
nrf_delay_ms(500);
nrf_gpio_pin_clear(LED_0);
nrf_delay_ms(500);
}
}
nrf_gpio_pin_set(LED_1);
nrf_gpio_pin_clear(LED_0);
who_am_i = read_register(MMA8452_ADDRESS << 1,WHO_AM_I,&data);
if(who_am_i){ //data == 0x2A
nrf_delay_us(100);
write_register(MMA8452_ADDRESS << 1,XYZ_DATA_CFG, 0x02);
nrf_delay_us(100);
write_register(MMA8452_ADDRESS << 1,CTRL_REG1, 0x33);
nrf_gpio_pin_clear(LED_1);
nrf_gpio_pin_set(LED_0);
char c[30];
sprintf(c,"%d",data);
simple_uart_putstring((const uint8_t *) "\r\n WHO_AM_I : ");
simple_uart_putstring((const uint8_t *) c);
data = '0';
simple_uart_putstring((const uint8_t *) "\r\nMMA8452Q is online...");
}
else{
nrf_gpio_pin_clear(LED_0);
nrf_gpio_pin_clear(LED_1);
simple_uart_putstring((const uint8_t *) "\r\nMMA8452Q is offline...");
}
short int x[3];
while(1){
readAccelData(x);
float accelG[3]; // Stores the real accel value in g's
for (int i = 0 ; i < 3 ; i++)
{
// accelG[i] = (float) x[i] / ((1<<12)/(2*GSCALE)); // get actual g value, this depends on scale being set
accelG[i] = (float) x[i] / 256; // get actual g value, this depends on scale being set
accelG[i] *= 10;
}
// int a = accelG[1]*10;
// int b = accelG[2]*10;
//// parse(a,b);
//
char c[30];
sprintf(c,"%d",x[0]);
simple_uart_putstring((const uint8_t *) "\r\n");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",x[1]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",x[2]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
}
}
void updateYPR(void) {
int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius
float temperature;
// If intPin goes high, all data registers have new data
if (MPU9250_I2C_ByteRead(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
// Now we'll calculate the accleration value into actual g's
ax = (float) accelCount[0] * aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float) accelCount[1] * aRes - accelBias[1];
az = (float) accelCount[2] * aRes - accelBias[2];
readGyroData(gyroCount); // Read the x/y/z adc values
// Calculate the gyro value into actual degrees per second
gx = (float) gyroCount[0] * gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
gy = (float) gyroCount[1] * gRes - gyroBias[1];
gz = (float) gyroCount[2] * gRes - gyroBias[2];
readMagData(magCount); // Read the x/y/z adc values
// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float) magCount[0] * mRes * magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float) magCount[1] * mRes * magCalibration[1] - magbias[1];
mz = (float) magCount[2] * mRes * magCalibration[2] - magbias[2];
Now = timer_get_us();
deltat = (float) ((Now - lastUpdate) / 1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;
sum += deltat;
sumCount++;
//MadgwickQuaternionUpdate(ax, ay, az, gx*_PI/180.0f, gy*_PI/180.0f, gz*_PI/180.0f, my, mx, mz);
MahonyQuaternionUpdate(ax, ay, az, gx * _PI / 180.0f, gy * _PI / 180.0f, gz * _PI / 180.0f, my, mx, mz);
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
ypr[0] = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]),
q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
// q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
// ypr[0] = atan2(2.0f * (q[1] * q[2] - q[0] * q[3]),
// q[0] * q[0] + q[1] * q[1] - 1);
ypr[1] = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
ypr[2] = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),
q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
ypr[1] *= 180.0f / _PI;
ypr[0] *= 180.0f / _PI;
ypr[0] -= -5.08f; // Declination at Hefei, Anhui is 5 degrees and 5 minutes (negative) on 2015-08-08
ypr[2] *= 180.0f / _PI;
}
// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = systemTime - count;
if (delt_t > 1500) { // update LCD once per half-second independent of read rate
trace_printf("ax = %f", 1000 * ax);
trace_printf(" ay = %f", 1000 * ay);
trace_printf(" az = %f mg\n", 1000 * az);
trace_printf("gx = %f", gx);
trace_printf(" gy = %f", gy);
trace_printf(" gz = %f deg/s\n", gz);
trace_printf("gx = %f", mx);
trace_printf(" gy = %f", my);
trace_printf(" gz = %f mG\n", mz);
tempCount = readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
trace_printf("temperature = %f C\n", temperature);
trace_printf("q0 = %f\n", q[0]);
trace_printf("q1 = %f\n", q[1]);
trace_printf("q2 = %f\n", q[2]);
trace_printf("q3 = %f\n", q[3]);
trace_printf("Yaw, Pitch, Roll: %f %f %f\n", ypr[0], ypr[1], ypr[2]);
trace_printf("average rate = %f\n\n\n\r", (float) sumCount / sum);
count = systemTime;
if (count > 1 << 21) {
systemTime = 0; // start the timer over again if ~30 minutes has passed
count = 0;
deltat = 0;
lastUpdate = timer_get_us();
}
sum = 0;
sumCount = 0;
}
}
float SNShield::getAccz(){
float acc[3];
readAccelData(acc);
this->accz = acc[2];
return acc[2];
}
float SNShield::getAccy(){
float acc[3];
readAccelData(acc);
this->accy = acc[1];
return acc[1];
}
float SNShield::getAccx(){
float acc[3];
readAccelData(acc);
this->accx = acc[0];
return acc[0];
}
