void register_write_memory(void *opaque, hwaddr addr,
uint64_t value, unsigned size)
{
RegisterInfoArray *reg_array = opaque;
RegisterInfo *reg = NULL;
uint64_t we;
int i;
for (i = 0; i < reg_array->num_elements; i++) {
if (reg_array->r[i]->access->addr == addr) {
reg = reg_array->r[i];
break;
}
}
if (!reg) {
qemu_log_mask(LOG_GUEST_ERROR, "Write to unimplemented register at " \
"address: %#" PRIx64 "\n", addr);
return;
}
/* Generate appropriate write enable mask */
if (reg->data_size < size) {
we = MAKE_64BIT_MASK(0, reg->data_size * 8);
} else {
we = MAKE_64BIT_MASK(0, size * 8);
}
register_write(reg, value, we, reg_array->prefix,
reg_array->debug);
}
/*
set the DLPF filter frequency. Assumes caller has taken semaphore
*/
void AP_InertialSensor_MPU6000::_set_filter_register(uint8_t filter_hz, uint8_t default_filter)
{
uint8_t filter = default_filter;
// choose filtering frequency
switch (filter_hz) {
case 5:
filter = BITS_DLPF_CFG_5HZ;
break;
case 10:
filter = BITS_DLPF_CFG_10HZ;
break;
case 20:
filter = BITS_DLPF_CFG_20HZ;
break;
case 42:
filter = BITS_DLPF_CFG_42HZ;
break;
case 98:
filter = BITS_DLPF_CFG_98HZ;
break;
}
if (filter != 0) {
_last_filter_hz = filter_hz;
register_write(MPUREG_CONFIG, filter);
}
}
static inline void
far_increment_mna(bitstream_parser_t *bitstream) {
const id_vlx_t chiptype = bitstream->type;
sw_far_t far;
fill_swfar(&far, register_read(bitstream, FAR));
_far_increment_mna(chiptype, &far);
register_write(bitstream, FAR, get_hwfar(&far));
}
// Update gyro offsets with new values. New offset values are substracted to actual offset values.
// offset values in gyro LSB units (as read from registers)
void MPU6000::set_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
{
int16_t aux_int;
if (offsetX != 0){
// Read actual value
aux_int = (register_read(MPUREG_XG_OFFS_USRH)<<8) | register_read(MPUREG_XG_OFFS_USRL);
aux_int -= offsetX<<1; // Adjust to internal units
// Write to MPU registers
register_write(MPUREG_XG_OFFS_USRH, (aux_int>>8)&0xFF);
register_write(MPUREG_XG_OFFS_USRL, aux_int&0xFF);
}
static void rpu_write(void *opaque, hwaddr addr, uint64_t value,
unsigned size)
{
RPU *s = XILINX_RPU(opaque);
RegisterInfo *r = &s->regs_info[addr / 4];
if (!r->data) {
qemu_log("%s: Decode error: write to %" HWADDR_PRIx "=%" PRIx64 "\n",
object_get_canonical_path(OBJECT(s)),
addr, value);
return;
}
register_write(r, value, ~0);
}
static void gic_proxy_write(void *opaque, hwaddr addr, uint64_t value,
unsigned size)
{
GICProxy *s = XILINX_GIC_PROXY(opaque);
RegisterInfo *r = &s->regs_info[addr / 4];
if (!r->data) {
qemu_log_mask(LOG_GUEST_ERROR,
"%s: Decode error: write %" HWADDR_PRIx "=%" PRIx64 "\n",
object_get_canonical_path(OBJECT(s)),
addr, value);
return;
}
register_write(r, value, ~0);
}
void AP_InertialSensor_MPU6000::hardware_init()
{
// MPU6000 chip select setup
pinMode(_cs_pin, OUTPUT);
digitalWrite(_cs_pin, HIGH);
delay(1);
// Chip reset
register_write(MPUREG_PWR_MGMT_1, BIT_H_RESET);
delay(100);
// Wake up device and select GyroZ clock (better performance)
register_write(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_I2C_IF_DIS);
delay(1);
// SAMPLE RATE
register_write(MPUREG_SMPLRT_DIV,0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
delay(1);
// FS & DLPF FS=2000º/s, DLPF = 98Hz (low pass filter)
register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
delay(1);
register_write(MPUREG_GYRO_CONFIG,BITS_FS_2000DPS); // Gyro scale 2000º/s
delay(1);
register_write(MPUREG_ACCEL_CONFIG,0x08); // Accel scele 4g (4096LSB/g)
delay(1);
// INT CFG => Interrupt on Data Ready
register_write(MPUREG_INT_ENABLE,BIT_RAW_RDY_EN); // INT: Raw data ready
delay(1);
register_write(MPUREG_INT_PIN_CFG,BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
delay(1);
// Oscillator set
// register_write(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
delay(1);
attachInterrupt(6,data_interrupt,RISING);
}
static gint
handle_cmd_write(bitstream_parsed_t *parsed,
bitstream_parser_t *parser) {
cmd_code_t cmd = register_read(parser, CMD);
switch(cmd) {
case MFW:
debit_log(L_BITSTREAM,"Executing multi-frame write");
record_frame(parsed, parser, register_read(parser, FAR));
break;
case RCRC:
debit_log(L_BITSTREAM,"Resetting CRC");
register_write(parser, CRC, 0);
break;
default:
debit_log(L_BITSTREAM,"execution of %i:%s is a noop",
cmd, cmd_names[cmd]);
break;
}
return 0;
}
void MPU6000::hardware_init()
{
// MPU6000 chip select setup
pinMode(_cs_pin, OUTPUT);
digitalWrite(_cs_pin, HIGH);
delay(1);
// Chip reset
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
delay(100);
// Wake up device and select GyroZ clock (better performance)
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
delay(1);
register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
delay(1);
// SAMPLE RATE
register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
delay(1);
// FS & DLPF FS=2000º/s, DLPF = 98Hz (low pass filter)
register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
delay(1);
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
delay(1);
_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
register_write(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
delay(1);
register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // configure interrupt to fire when new data arrives
delay(1);
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR); // clear interrupt on any read
delay(1);
attachInterrupt(6,data_interrupt,RISING);
// initialise DMP. Should we only do this when we know we want to use the DMP for attitude sensing as well?
dmp_init();
}
void io_write_changed(uint8_t reg)
{
if (io_reg_buffer[reg] != reg_mirror[reg]) {
if (reg == 0x03 || reg == 0x07) {
/* Trick for avoiding phase reset when changing high bits of timer period */
if ((reg_mirror[reg] & 0x07) - (io_reg_buffer[reg] & 0x07) == 1) {
uint8_t low_val = reg_mirror[0x02];
register_write(0x02, 0); // low value = 0
register_write(0x01, 0x8F); // enable sweep, negate, shift = 7
register_write(0x17, 0xC0); // clock sweep immediately
register_write(0x01, 0x0F); // disable sweep
register_write(0x02, low_val); // put back low value
reg_mirror[0x02] = low_val;
reg_mirror[reg] = io_reg_buffer[reg];
}
else if ((io_reg_buffer[reg] & 0x07) - (reg_mirror[reg] & 0x07) == 1) {
uint8_t low_val = reg_mirror[0x02];
register_write(0x02, 0xFF);
register_write(0x01, 0x87); // enable sweep, negate, shift = 7
register_write(0x17, 0xC0); // clock sweep immediately
register_write(0x01, 0x0F); // disable sweep
register_write(0x02, low_val); // put back low value
reg_mirror[0x02] = low_val;
reg_mirror[reg] = io_reg_buffer[reg];
}
else
register_write(reg, io_reg_buffer[reg]);
}
else
register_write(reg, io_reg_buffer[reg]);
// reg_mirror[reg] = io_reg_buffer[reg];
}
}
void io_register_write(uint8_t reg, uint8_t value)
{
register_write(reg, value);
}
bool AP_InertialSensor_MPU6000::hardware_init(Sample_rate sample_rate)
{
if (!_spi_sem->take(100)) {
hal.scheduler->panic(PSTR("MPU6000: Unable to get semaphore"));
}
// Chip reset
uint8_t tries;
for (tries = 0; tries<5; tries++) {
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
hal.scheduler->delay(100);
// Wake up device and select GyroZ clock. Note that the
// MPU6000 starts up in sleep mode, and it can take some time
// for it to come out of sleep
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
hal.scheduler->delay(5);
// check it has woken up
if (_register_read(MPUREG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_ZGYRO) {
break;
}
#if MPU6000_DEBUG
_dump_registers();
#endif
}
if (tries == 5) {
hal.console->println_P(PSTR("Failed to boot MPU6000 5 times"));
_spi_sem->give();
return false;
}
register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
hal.scheduler->delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
hal.scheduler->delay(1);
uint8_t default_filter;
// sample rate and filtering
// to minimise the effects of aliasing we choose a filter
// that is less than half of the sample rate
switch (sample_rate) {
case RATE_50HZ:
// this is used for plane and rover, where noise resistance is
// more important than update rate. Tests on an aerobatic plane
// show that 10Hz is fine, and makes it very noise resistant
default_filter = BITS_DLPF_CFG_10HZ;
_sample_shift = 2;
break;
case RATE_100HZ:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 1;
break;
case RATE_200HZ:
default:
default_filter = BITS_DLPF_CFG_20HZ;
_sample_shift = 0;
break;
}
_set_filter_register(_mpu6000_filter, default_filter);
// set sample rate to 200Hz, and use _sample_divider to give
// the requested rate to the application
register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ);
hal.scheduler->delay(1);
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d
_mpu6000_product_id = _register_read(MPUREG_PRODUCT_ID);
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _mpu6000_product_id);
if ((_mpu6000_product_id == MPU6000ES_REV_C4) || (_mpu6000_product_id == MPU6000ES_REV_C5) ||
(_mpu6000_product_id == MPU6000_REV_C4) || (_mpu6000_product_id == MPU6000_REV_C5)) {
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
register_write(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives
register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
hal.scheduler->delay(1);
// clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN);
hal.scheduler->delay(1);
_spi_sem->give();
return true;
}
