/*******************************************************************************
* Function Name : Axis3_Test
* Description : Light Sensor Test.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void Axis3_Test(void)
{
char buf[24];
int32_t xoff = 0;
int32_t yoff = 0;
int32_t zoff = 0;
int8_t x = 0;
int8_t y = 0;
int8_t z = 0;
OLED_ClearScreen();
OLED_DisStrLine(0, 0, "Axis-3");
I2CInit(I2CMASTER, 0);
acc_init();
/* Assume base board in zero-g position when reading first value. */
acc_read(&x, &y, &z);
xoff = 0-x;
yoff = 0-y;
zoff = 0-z;
// while(1)
// {
/* Accelerometer */
acc_read(&x, &y, &z);
x = x+xoff;
y = y+yoff;
z = z+zoff;
snprintf(buf, 20, "Acc x: %d ", x);
OLED_DisStrLine(2, 0, (uint8_t *)buf);
printf("\r\nAcc x: %d, ", x);
snprintf(buf, 20, "Acc y: %d ", y);
OLED_DisStrLine(3, 0, (uint8_t *)buf);
printf("Acc y: %d, ", y);
snprintf(buf, 20, "Acc z: %d ", z);
OLED_DisStrLine(4, 0, (uint8_t *)buf);
printf("Acc z: %d. ", z);
delay_ms(250);
// if(KEY_Read() == KEY_ESC)
// break;
// }
}
uint16_t acc_magnitude(int8_t* angle)
{
int16_t acc[3], a, alpha;
/* briefly turn on accelerometer */
acc_write(ACC_REG_CTRL_REG1, 0x97);
timer_wait(MILLISECONDS(2));
acc_read(ACC_REG_OUT_X, sizeof(acc), (uint8_t*)&acc);
acc_write(ACC_REG_CTRL_REG1, 0x00);
/* get acceleration vector magnitude */
a = sqrt32(
((int)acc[0])*acc[0] +
((int)acc[1])*acc[1] +
((int)acc[2])*acc[2]
)/64;
/* calculate tag angle */
if(angle)
{
if(!a)
alpha = 127;
else
{
alpha = (acc[2]*2)/a;
if(alpha>127)
alpha=127;
else
if(alpha<-127)
alpha=-127;
}
*angle = asin7deg(alpha);
}
return a;
}
/**
* Appends data at the input-buffer of the channel.
* The expected number of bytes is passed to the function. Data are read
* from chat_channel::fd and inserted into achat_channel::recvbuffer.
*
* @param acc The channel
* @param size Number of bytes top read from the filedescriptor
* @return If the number of expected bytes were read from the filedescriptor
* achat_rc::ACHAT_RC_OK is returned. If the number of bytes read from
* the filedescriptor is less than <code>size</code>
* achat_rc::ACHAT_RC_PENDING is returned.
*/
static achat_rc
acc_fillrecvbuffer(struct achat_channel *acc, size_t size)
{
const size_t bsize = acc_bufferlen(acc->recvbuffer);
ssize_t nread;
char *readbuf;
int mincnt = size;
int needcnt;
int error;
if (acc->blocking == ACC_NON_BLOCKING) {
/* Read at least 4k */
if (mincnt < 4096)
mincnt = 4096;
}
ACC_CHKPARAM(mincnt <= ACHAT_MAX_MSGSIZE);
ACC_CHKPARAM(mincnt >= 0);
needcnt = mincnt - bsize;
if (needcnt <= 0) {
/* You have enough data in your buffer */
return (ACHAT_RC_OK);
}
/* Make space in receive-buffer */
readbuf = acc_bufferappend_space(acc->recvbuffer, needcnt);
if (readbuf == NULL)
return (ACHAT_RC_ERROR);
nread = acc_read(acc->fd, readbuf, needcnt);
error = errno;
if (nread < 0) {
/* Buffer not filled, remove allocated space again */
achat_rc rc = acc_buffertrunc(acc->recvbuffer, needcnt);
if (rc != ACHAT_RC_OK)
return (ACHAT_RC_ERROR);
return (error == EAGAIN) ? ACHAT_RC_PENDING : ACHAT_RC_ERROR;
}
if (nread < needcnt) {
/* nread might be < needcnt, truncate the buffer to have the */
/* correct buffer size */
achat_rc rc = acc_buffertrunc(acc->recvbuffer, needcnt - nread);
if (rc != ACHAT_RC_OK)
return (ACHAT_RC_ERROR);
}
return (nread > 0) ? ACHAT_RC_OK : ACHAT_RC_EOF;
}
uint8_t acc_init(void)
{
int i;
uint8_t data;
/* initialize GPIO */
nrf_gpio_cfg_input(CONFIG_ACC_INT1, NRF_GPIO_PIN_NOPULL);
nrf_gpio_cfg_input(CONFIG_ACC_MISO, NRF_GPIO_PIN_NOPULL);
nrf_gpio_cfg_output(CONFIG_ACC_MOSI);
nrf_gpio_cfg_output(CONFIG_ACC_SCK);
nrf_gpio_cfg_output(CONFIG_ACC_nCS);
/* mark inactive by default */
nrf_gpio_pin_set(CONFIG_ACC_nCS);
/* configure peripheral */
SPI_ACC->PSELMISO = CONFIG_ACC_MISO;
SPI_ACC->PSELMOSI = CONFIG_ACC_MOSI;
SPI_ACC->PSELSCK = CONFIG_ACC_SCK;
/* configure accelerometer for 8MHz */
SPI_ACC->FREQUENCY = SPI_FREQUENCY_FREQUENCY_M8;
SPI_ACC->CONFIG =
(SPI_CONFIG_ORDER_MsbFirst << SPI_CONFIG_ORDER_Pos) |\
(SPI_CONFIG_CPHA_Trailing << SPI_CONFIG_CPHA_Pos) |\
(SPI_CONFIG_CPOL_ActiveLow << SPI_CONFIG_CPOL_Pos);
/* reset events */
SPI_ACC->EVENTS_READY = 0U;
/* enable SPI accelerometer peripheral */
SPI_ACC->ENABLE =
(SPI_ENABLE_ENABLE_Enabled << SPI_ENABLE_ENABLE_Pos);
/* check accelerometer read */
acc_read(ACC_REG_WHO_AM_I, sizeof(data), &data);
if(data!=0x33)
return 1;
/* initialize accelerometer */
for(i=0; i<ACC_INIT_COUNT; i++)
acc_write(g_acc_init[i][0], g_acc_init[i][1]);
return 0;
}
/**
* \brief Returns a reading from the sensor
* \param type MPU_9250_SENSOR_TYPE_ACC_[XYZ] or MPU_9250_SENSOR_TYPE_GYRO_[XYZ]
* \return centi-G (ACC) or centi-Deg/Sec (Gyro)
*/
static int
value(int type)
{
int rv;
float converted_val = 0;
if(state == SENSOR_STATE_DISABLED) {
PRINTF("MPU: Sensor Disabled\n");
return CC26XX_SENSOR_READING_ERROR;
}
memset(sensor_value, 0, sizeof(sensor_value));
if((type & MPU_9250_SENSOR_TYPE_ACC) != 0) {
t0 = RTIMER_NOW();
while(!int_status() &&
(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));
rv = acc_read(sensor_value);
if(rv == 0) {
return CC26XX_SENSOR_READING_ERROR;
}
PRINTF("MPU: ACC = 0x%04x 0x%04x 0x%04x = ",
sensor_value[0], sensor_value[1], sensor_value[2]);
/* Convert */
if(type == MPU_9250_SENSOR_TYPE_ACC_X) {
converted_val = acc_convert(sensor_value[0]);
} else if(type == MPU_9250_SENSOR_TYPE_ACC_Y) {
converted_val = acc_convert(sensor_value[1]);
} else if(type == MPU_9250_SENSOR_TYPE_ACC_Z) {
converted_val = acc_convert(sensor_value[2]);
}
rv = (int)(converted_val * 100);
} else if((type & MPU_9250_SENSOR_TYPE_GYRO) != 0) {
t0 = RTIMER_NOW();
while(!int_status() &&
(RTIMER_CLOCK_LT(RTIMER_NOW(), t0 + READING_WAIT_TIMEOUT)));
rv = gyro_read(sensor_value);
if(rv == 0) {
return CC26XX_SENSOR_READING_ERROR;
}
PRINTF("MPU: Gyro = 0x%04x 0x%04x 0x%04x = ",
sensor_value[0], sensor_value[1], sensor_value[2]);
if(type == MPU_9250_SENSOR_TYPE_GYRO_X) {
converted_val = gyro_convert(sensor_value[0]);
} else if(type == MPU_9250_SENSOR_TYPE_GYRO_Y) {
converted_val = gyro_convert(sensor_value[1]);
} else if(type == MPU_9250_SENSOR_TYPE_GYRO_Z) {
converted_val = gyro_convert(sensor_value[2]);
}
rv = (int)(converted_val * 100);
} else {
PRINTF("MPU: Invalid type\n");
rv = CC26XX_SENSOR_READING_ERROR;
}
PRINTF("%ld\n", (long int)(converted_val * 100));
return rv;
}