// Calculates the current percentage (actual value is 0-1) of
// the mpu's fifo that is filled
float mpu_fifo_capacity(Sensor *s)
{
// Get the current data count
int fifo_count = (FETCH_REG(MPU_FIFO_COUNTH) << 8) +
FETCH_REG(MPU_FIFO_COUNTL);
// Return used / fifo capacity
return ((float)fifo_count / 1024);
}
// Configure fifo settings
// i2c_slv_addr: 0xN
// clkout_en: (on|off)
// aux_burst_addr: 0xN
uint8_t itg_config_aux(Sensor *s, KeyVal * pairs)
{
// Fetch the current register value
uint8_t byte = FETCH_REG(ITG_AUX_SLV_ADDR),
reg_val = byte;
// Keep track of register changes
int applied = 0;
// Iterate through the settings
do {
if (!pairs->applied)
{
pairs->applied = 1;
// If the i2c_slv_addr key
if (!strcmp(pairs->key, "i2c_slv_addr"))
{
// Configure the aux i2c slave addr
process_i2c_slv_addr_key(®_val, pairs->val);
}
// Else if the clkout_en key
else if (!strcmp(pairs->key, "clkout_en"))
{
// Configure the reference clock out
process_clkout_en_key(®_val, pairs->val);
}
// Else if the burst addr
else if (!strcmp(pairs->key, "aux_burst_addr"))
{
// Configure the auxiliary device burst address
uint8_t addr = 0xff & DEX_TO_INT(pairs->val);
SET_REG(ITG_AUX_ADDR, addr);
applied++;
}
// Else not supported key
else
{
// So set applied to false
pairs->applied = 0;
}
}
} while ((pairs = pairs->next));
// If the new reg val is different from the current
if (reg_val != byte)
{
// Set the register value to reg_val
SET_REG(ITG_AUX_SLV_ADDR, reg_val);
// Return 1 to signify applied change
applied++;
}
// Return number of applied changes
return applied;
}
static Axes *read_burst(Sensor *s)
{
// Get the current data count
int fifo_count = (FETCH_REG(MPU_FIFO_COUNTH) << 8) +
FETCH_REG(MPU_FIFO_COUNTL);
// Return null if currently empty
if (!fifo_count)
{
// Return null
return NULL;
}
// Adjust count to a multiple of three
fifo_count = (fifo_count / 6) * 6;
// Otherwise generate an array of uint8_ts
uint8_t *block = i2c_read_block( s->i2c,
s->i2c_addr,
MPU_FIFO_R_W,
fifo_count ),
*data = (block + fifo_count);
// Create pointers to Axes
Axes *head = NULL;
// Parse into axes data
for (int i = fifo_count / 6; i > 0; i--)
{
// malloc next link with the previous head as its next
head = axes_malloc(head);
// Decrement data
data -= 6;
// Extract x y z values
head->x = (data[0] << 8) | data[1];
head->y = (data[2] << 8) | data[3];
head->z = (data[4] << 8) | data[5];
}
// Free the byte array
free(block);
// Return the Axes
return head;
}
void i386_next_fetch_sp_registers (unsigned char *rdata, gdb_i386_thread_state_t *sp_regs)
{
/* these need to match the REGISTER_NAMES table from tm-i386.h */
FETCH_REG (rdata, 0, sp_regs->eax);
FETCH_REG (rdata, 1, sp_regs->ecx);
FETCH_REG (rdata, 2, sp_regs->edx);
FETCH_REG (rdata, 3, sp_regs->ebx);
FETCH_REG (rdata, 4, sp_regs->esp);
FETCH_REG (rdata, 5, sp_regs->ebp);
FETCH_REG (rdata, 6, sp_regs->esi);
FETCH_REG (rdata, 7, sp_regs->edi);
FETCH_REG (rdata, 8, sp_regs->eip);
FETCH_REG (rdata, 9, sp_regs->eflags);
FETCH_REG (rdata, 10, sp_regs->cs);
FETCH_REG (rdata, 11, sp_regs->ss);
FETCH_REG (rdata, 12, sp_regs->ds);
FETCH_REG (rdata, 13, sp_regs->es);
FETCH_REG (rdata, 14, sp_regs->fs);
FETCH_REG (rdata, 15, sp_regs->gs);
}
