Ejemplo n.º 1
0
bool icm20689SpiDetect(const busDevice_t *bus)
{
    icm20689SpiInit(bus);

    spiSetDivisor(bus->spi.instance, SPI_CLOCK_INITIALIZATON); //low speed

    spiWriteRegister(bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);

    uint8_t attemptsRemaining = 20;
    do {
        delay(150);
        const uint8_t whoAmI = spiReadRegister(bus, MPU_RA_WHO_AM_I);
        if (whoAmI == ICM20689_WHO_AM_I_CONST) {
            break;
        }
        if (!attemptsRemaining) {
            return false;
        }
    } while (attemptsRemaining--);

    spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);

    return true;

}
Ejemplo n.º 2
0
/**
 * Initialize the driver, must be called before any other routines.
 *
 * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
 * m25p16_getGeometry().
 */
bool m25p16_init()
{
    //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
#ifdef STM32F40_41xxx
    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_21MHZ_CLOCK_DIVIDER);
#else
    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);
#endif

    return m25p16_readIdentification();
}
Ejemplo n.º 3
0
static void mpu6000AccAndGyroInit(void) {

    if (mpuSpi6000InitDone) {
        return;
    }

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    // Device Reset
    mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
    delay(150);

    mpu6000WriteRegister(MPU_RA_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
    delay(150);

    // Clock Source PPL with Z axis gyro reference
    mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
    delayMicroseconds(15);

    // Disable Primary I2C Interface
    mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS);
    delayMicroseconds(15);

    mpu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00);
    delayMicroseconds(15);

    // Accel Sample Rate 1kHz
    // Gyroscope Output Rate =  1kHz when the DLPF is enabled
    mpu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
    delayMicroseconds(15);

    // Gyro +/- 1000 DPS Full Scale
    mpu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
    delayMicroseconds(15);

    // Accel +/- 8 G Full Scale
    mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
    delayMicroseconds(15);


    mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
    delayMicroseconds(15);

#ifdef USE_MPU_DATA_READY_SIGNAL
    mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
    delayMicroseconds(15);
#endif

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock
    delayMicroseconds(1);

    mpuSpi6000InitDone = true;
}
Ejemplo n.º 4
0
void max7456Init(const vcdProfile_t *pVcdProfile)
{
#ifdef MAX7456_SPI_CS_PIN
    max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
    IOInit(max7456CsPin, OWNER_OSD_CS, 0);
    IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);

    spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
    // force soft reset on Max7456
    ENABLE_MAX7456;
    max7456Send(MAX7456ADD_VM0, MAX7456_RESET);
    DISABLE_MAX7456;

    // Setup values to write to registers
    videoSignalCfg = pVcdProfile->video_system;
    hosRegValue = 32 - pVcdProfile->h_offset;
    vosRegValue = 16 - pVcdProfile->v_offset;

#ifdef MAX7456_DMA_CHANNEL_TX
    dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif

    // Real init will be made later when driver detect idle.
}
Ejemplo n.º 5
0
/**
 * Initialize the driver, must be called before any other routines.
 *
 * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
 * m25p16_getGeometry().
 */
bool m25p16_init(ioTag_t csTag)
{
    /*
        if we have already detected a flash device we can simply exit

        TODO: change the init param in favour of flash CFG when ParamGroups work is done
        then cs pin can be specified in hardware_revision.c or config.c (dependent on revision).
    */
    if (geometry.sectors) {
        return true;
    }

    if (csTag) {
        m25p16CsPin = IOGetByTag(csTag);
    } else {
#ifdef M25P16_CS_PIN
        m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
#else
        return false;
#endif
    }
    IOInit(m25p16CsPin, OWNER_FLASH_CS, 0);
    IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG);

    DISABLE_M25P16;

#ifndef M25P16_SPI_SHARED
    //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST);
#endif

    return m25p16_readIdentification();
}
Ejemplo n.º 6
0
/**
 * Initialize the driver, must be called before any other routines.
 *
 * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
 * m25p16_getGeometry().
 */
bool m25p16_init()
{
    //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
    spiSetDivisor(M25P16_SPI_INSTANCE, HIGH_SPEED_SPI);

    return m25p16_readIdentification();
}
Ejemplo n.º 7
0
bool mpu6000SpiGyroDetect(const mpu6000Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
{
    mpu6000Config = configToUse;

    if (!mpu6000SpiDetect()) {
        return false;
    }

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    mpu6000AccAndGyroInit();

    uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
    int16_t data[3];

    // default lpf is 42Hz
    if (lpf == 256)
        mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
    else if (lpf >= 188)
        mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
    else if (lpf >= 98)
        mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
    else if (lpf >= 42)
        mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
    else if (lpf >= 20)
        mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
    else if (lpf >= 10)
        mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
    else if (lpf > 0)
        mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
    else
        mpuLowPassFilter = BITS_DLPF_CFG_256HZ;

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    // Determine the new sample divider
    mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
    delayMicroseconds(1);

    // Accel and Gyro DLPF Setting
    mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
    delayMicroseconds(1);

    mpu6000SpiGyroRead(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
        return false;
    }
    gyro->init = mpu6000SpiGyroInit;
    gyro->read = mpu6000SpiGyroRead;
    gyro->intStatus = checkMPU6000DataReady;
    // 16.4 dps/lsb scalefactor
    gyro->scale = 1.0f / 16.4f;
    //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
    delay(100);
    return true;
}
Ejemplo n.º 8
0
static void hmc5883SpiInit(busDevice_t *busdev)
{
    IOHi(busdev->busdev_u.spi.csnPin); // Disable

    IOInit(busdev->busdev_u.spi.csnPin, OWNER_COMPASS_CS, 0);
    IOConfigGPIO(busdev->busdev_u.spi.csnPin, IOCFG_OUT_PP);

    spiSetDivisor(busdev->busdev_u.spi.instance, SPI_CLOCK_STANDARD);
}
Ejemplo n.º 9
0
/**
 * Set a band and channel
 */
void rtc6705SetBandAndChannel(uint8_t band, uint8_t channel)
{
    band = constrain(band, 0, VTX_RTC6705_BAND_COUNT - 1);
    channel = constrain(channel, 0, VTX_RTC6705_CHANNEL_COUNT - 1);

    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);

    rtc6705Transfer(RTC6705_SET_HEAD);
    rtc6705Transfer(channelArray[band][channel]);
}
Ejemplo n.º 10
0
static void mpu6500SpiInit(void)
{
    static bool hardwareInitialised = false;

    if (hardwareInitialised) {
        return;
    }

	mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN));
    IOInit(mpuSpi6500CsPin, OWNER_SYSTEM, RESOURCE_SPI);
	IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);

#if defined(STM32F40_41xxx) || defined (STM32F411xE)
    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_SLOW_CLOCK);
#else
    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_STANDARD_CLOCK);
#endif

    hardwareInitialised = true;
}
Ejemplo n.º 11
0
void mpu6000SpiAccRead(int16_t *gyroData)
{
    uint8_t buf[6];

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock

    mpu6000ReadRegister(MPU6000_ACCEL_XOUT_H, buf, 6);

    gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
    gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
    gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
}
Ejemplo n.º 12
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
    mpuIntExtiInit();

    mpu6000AccAndGyroInit();

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    // Accel and Gyro DLPF Setting
    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
    delayMicroseconds(1);

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER);  // 18 MHz SPI clock

    int16_t data[3];
    mpuGyroRead(data);

    if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}
Ejemplo n.º 13
0
static void mpu6000AccAndGyroInit(uint8_t lpf) {

	if (mpuSpi6000InitDone) {
		return;
	}

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed for writing to slow registers

    mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
	delay(50);
	mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);
	delay(50);

	verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x0B); //temp sensor disabled Z axis is timer

    // Disable Primary I2C Interface
	mpu6000WriteRegister(MPU_RA_USER_CTRL, BIT_I2C_IF_DIS|5);

    verifympu6000WriteRegister(MPU_RA_PWR_MGMT_2, 0x00);

    // Accel Sample Rate 1kHz
    // Gyroscope Output Rate =  1kHz when the DLPF is enabled
    verifympu6000WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());

    // Gyro +/- 1000 DPS Full Scale
    verifympu6000WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);

    // Accel +/- 8 G Full Scale
    verifympu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);

    verifympu6000WriteRegister(MPU_RA_INT_PIN_CFG,  0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR

#if defined(USE_MPU_DATA_READY_SIGNAL)
    mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
    delayMicroseconds(100);
	verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x0B); //need to redo MPU_RA_PWR_MGMT_1
    verifympu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); //should work correctly this time
#endif

    // Accel and Gyro DLPF Setting
    if (lpf == 4) {
    	verifympu6000WriteRegister(MPU6000_CONFIG, 1); //1KHz, 188DLPF
    } else if (lpf < 4) {
    	verifympu6000WriteRegister(MPU6000_CONFIG, 7); //8KHz, Raw
    } else if (lpf > 4) {
    	verifympu6000WriteRegister(MPU6000_CONFIG, 0); //8KHz, 256DLPF
    }

    // Clock Source PPL with Z axis gyro reference
    verifympu6000WriteRegister(MPU_RA_PWR_MGMT_1, 0x09);

    mpuSpi6000InitDone = true; //init done
}
Ejemplo n.º 14
0
void rtc6705SetRFPower(uint8_t rf_power)
{
    rf_power = constrain(rf_power, VTX_RTC6705_MIN_POWER, VTX_RTC6705_POWER_COUNT - 1);

    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);

    uint32_t val_hex = RTC6705_RW_CONTROL_BIT; // write
    val_hex |= RTC6705_ADDRESS; // address
    const uint32_t data = rf_power > 1 ? PA_CONTROL_DEFAULT : (PA_CONTROL_DEFAULT | PD_Q5G_MASK) & (~(PA5G_PW_MASK | PA5G_BS_MASK));
    val_hex |= data << 5; // 4 address bits and 1 rw bit.

    rtc6705Transfer(val_hex);
}
Ejemplo n.º 15
0
bool mpu6000SpiDetect(void)
{
    uint8_t in;
    uint8_t attemptsRemaining = 5;

#ifdef MPU6000_CS_PIN 
    mpuSpi6000CsPin = IOGetByTag(IO_TAG(MPU6000_CS_PIN));
#endif
    IOInit(mpuSpi6000CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
    IOConfigGPIO(mpuSpi6000CsPin, SPI_IO_CS_CFG);

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);

    mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);

    do {
        delay(150);

        mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
        if (in == MPU6000_WHO_AM_I_CONST) {
            break;
        }
        if (!attemptsRemaining) {
            return false;
        }
    } while (attemptsRemaining--);


    mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);

    /* look for a product ID we recognise */

    // verify product revision
    switch (in) {
        case MPU6000ES_REV_C4:
        case MPU6000ES_REV_C5:
        case MPU6000_REV_C4:
        case MPU6000_REV_C5:
        case MPU6000ES_REV_D6:
        case MPU6000ES_REV_D7:
        case MPU6000ES_REV_D8:
        case MPU6000_REV_D6:
        case MPU6000_REV_D7:
        case MPU6000_REV_D8:
        case MPU6000_REV_D9:
        case MPU6000_REV_D10:
            return true;
    }

    return false;
}
Ejemplo n.º 16
0
/**
 * Handle a failure of an SD card operation by resetting the card back to its initialization phase.
 *
 * Increments the failure counter, and when the failure threshold is reached, disables the card until
 * the next call to sdcard_init().
 */
static void sdcard_reset(void)
{
    if (sdcard.state >= SDCARD_STATE_READY) {
        spiSetDivisor(SDCARD_SPI_INSTANCE, SDCARD_SPI_INITIALIZATION_CLOCK_DIVIDER);
    }

    sdcard.failureCount++;
    if (sdcard.failureCount >= SDCARD_MAX_CONSECUTIVE_FAILURES) {
        sdcard.state = SDCARD_STATE_NOT_PRESENT;
    } else {
        sdcard.operationStartTime = millis();
        sdcard.state = SDCARD_STATE_RESET;
    }
}
Ejemplo n.º 17
0
static void mpu9250AccAndGyroInit(uint8_t lpf) {

	if (mpuSpi9250InitDone) {
		return;
	}

    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed for writing to slow registers

    mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
	delay(50);

	verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);

#if defined (REVONANO) || defined (SPARKY2) || defined(ALIENFLIGHTF4) || defined(BLUEJAYF4) || defined(VRCORE)
    //mpu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_8800_32); //Fchoice_b defaults to 00 which makes fchoice 11
	verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11

    if (lpf == 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
    } else if (lpf < 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
    } else if (lpf > 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
    }

	verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
#else
	verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3 | FCB_DISABLED); //Fchoice_b defaults to 00 which makes fchoice 11

    if (lpf == 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
    } else if (lpf < 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
    } else if (lpf > 4) {
    	verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
    }

	verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
#endif

	verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
	verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN

#if defined(USE_MPU_DATA_READY_SIGNAL)
	verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
#endif

    mpuSpi9250InitDone = true; //init done
}
Ejemplo n.º 18
0
void icm20689GyroInit(gyroDev_t *gyro)
{
    mpuGyroInit(gyro);

    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_INITIALIZATON);

    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
    delay(100);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SIGNAL_PATH_RESET, 0x03);
    delay(100);
//    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, 0);
//    delay(100);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
    delay(15);
    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_GYRO_CONFIG, raGyroConfigData);
    delay(15);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
    delay(15);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_CONFIG, gyro->lpf);
    delay(15);
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
    delay(100);

    // Data ready interrupt configuration
//    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_PIN_CFG, 0x10);  // INT_ANYRD_2CLEAR, BYPASS_EN

    delay(15);

#ifdef USE_MPU_DATA_READY_SIGNAL
    gyro->mpuConfiguration.writeFn(&gyro->bus, MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif

    spiSetDivisor(gyro->bus.spi.instance, SPI_CLOCK_STANDARD);
}
Ejemplo n.º 19
0
bool mpu6000SpiDetect(void)
{
    uint8_t in;
    uint8_t attemptsRemaining = 5;
    if (mpuSpi6000InitDone) {
        return true;
    }

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    mpu6000WriteRegister(MPU6000_PWR_MGMT_1, BIT_H_RESET);

    do {
        delay(150);

        mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
        if (in == MPU6000_WHO_AM_I_CONST) {
            break;
        }
        if (!attemptsRemaining) {
            return false;
        }
    } while (attemptsRemaining--);


    mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1);

    /* look for a product ID we recognise */

    // verify product revision
    switch (in) {
        case MPU6000ES_REV_C4:
        case MPU6000ES_REV_C5:
        case MPU6000_REV_C4:
        case MPU6000_REV_C5:
        case MPU6000ES_REV_D6:
        case MPU6000ES_REV_D7:
        case MPU6000ES_REV_D8:
        case MPU6000_REV_D6:
        case MPU6000_REV_D7:
        case MPU6000_REV_D8:
        case MPU6000_REV_D9:
        case MPU6000_REV_D10:
            return true;
    }

    return false;
}
Ejemplo n.º 20
0
static void mpu6500SpiInit(void)
{
    static bool hardwareInitialised = false;

    if (hardwareInitialised) {
        return;
    }

    mpuSpi6500CsPin = IOGetByTag(IO_TAG(MPU6500_CS_PIN));
    IOInit(mpuSpi6500CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
    IOConfigGPIO(mpuSpi6500CsPin, SPI_IO_CS_CFG);

    spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_FAST);

    hardwareInitialised = true;
}
Ejemplo n.º 21
0
static void icm20689SpiInit(const busDevice_t *bus)
{
    static bool hardwareInitialised = false;

    if (hardwareInitialised) {
        return;
    }

    IOInit(bus->spi.csnPin, OWNER_MPU_CS, 0);
    IOConfigGPIO(bus->spi.csnPin, SPI_IO_CS_CFG);
    IOHi(bus->spi.csnPin);

    spiSetDivisor(bus->spi.instance, SPI_CLOCK_STANDARD);

    hardwareInitialised = true;
}
Ejemplo n.º 22
0
void mpu6000AccAndGyroInit(void) {

    if (mpuSpi6000InitDone) {
        return;
    }

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);

    // Device Reset
    mpu6000WriteRegister(MPU6000_PWR_MGMT_1, BIT_H_RESET);
    delay(150);

    mpu6000WriteRegister(MPU6000_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
    delay(150);

    // Clock Source PPL with Z axis gyro reference
    mpu6000WriteRegister(MPU6000_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
    delayMicroseconds(1);

    // Disable Primary I2C Interface
    mpu6000WriteRegister(MPU6000_USER_CTRL, BIT_I2C_IF_DIS);
    delayMicroseconds(1);

    mpu6000WriteRegister(MPU6000_PWR_MGMT_2, 0x00);
    delayMicroseconds(1);

    // Accel Sample Rate 1kHz
    // Gyroscope Output Rate =  1kHz when the DLPF is enabled
    mpu6000WriteRegister(MPU6000_SMPLRT_DIV, 0x00);
    delayMicroseconds(1);

    // Accel +/- 8 G Full Scale
    mpu6000WriteRegister(MPU6000_ACCEL_CONFIG, BITS_FS_8G);
    delayMicroseconds(1);

    // Gyro +/- 1000 DPS Full Scale
    mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
    delayMicroseconds(1);

#ifdef USE_MPU_DATA_READY_SIGNAL
    // Set MPU Data Ready Signal
    mpu6000WriteRegister(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
    delayMicroseconds(1);
#endif

    mpuSpi6000InitDone = true;
}
Ejemplo n.º 23
0
 /**
 * Set a freq in mhz
 * Formula derived from datasheet
 */
void rtc6705SetFreq(uint16_t frequency)
{
    frequency = constrain(frequency, VTX_RTC6705_FREQ_MIN, VTX_RTC6705_FREQ_MAX);

    const uint32_t val_a = ((((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) % RTC6705_SET_FDIV) / RTC6705_SET_NDIV; //Casts required to make sure correct math (large numbers)
    const uint32_t val_n = (((uint64_t)frequency*(uint64_t)RTC6705_SET_DIVMULT*(uint64_t)RTC6705_SET_R)/(uint64_t)RTC6705_SET_DIVMULT) / RTC6705_SET_FDIV; //Casts required to make sure correct math (large numbers)

    uint32_t val_hex = RTC6705_SET_WRITE;
    val_hex |= (val_a << 5);
    val_hex |= (val_n << 12);

    spiSetDivisor(RTC6705_SPI_INSTANCE, SPI_CLOCK_SLOW);

    rtc6705Transfer(RTC6705_SET_HEAD);
    delayMicroseconds(10);
    rtc6705Transfer(val_hex);
}
Ejemplo n.º 24
0
bool mpu6000SpiDetect(void)
{
    uint8_t in;
    uint8_t attemptsRemaining = 5;

    spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI);

    mpu6000WriteRegister(MPU_RA_PWR_MGMT_1, BIT_H_RESET);

    return true;
    do {
        delay(150);

        mpu6000ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
        if (in == MPU6000_WHO_AM_I_CONST) {
            break;
        }
        if (!attemptsRemaining) {
            return false;
        }
    } while (attemptsRemaining--);


    mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);

    /* look for a product ID we recognise */

    // verify product revision
    switch (in) {
        case MPU6000ES_REV_C4:
        case MPU6000ES_REV_C5:
        case MPU6000_REV_C4:
        case MPU6000_REV_C5:
        case MPU6000ES_REV_D6:
        case MPU6000ES_REV_D7:
        case MPU6000ES_REV_D8:
        case MPU6000_REV_D6:
        case MPU6000_REV_D7:
        case MPU6000_REV_D8:
        case MPU6000_REV_D9:
        case MPU6000_REV_D10:
            return true;
    }

    return false;
}
Ejemplo n.º 25
0
/**
 * Initialize the driver, must be called before any other routines.
 *
 * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with
 * m25p16_getGeometry().
 */
bool m25p16_init()
{

#ifdef M25P16_CS_PIN 
    m25p16CsPin = IOGetByTag(IO_TAG(M25P16_CS_PIN));
#endif
    IOInit(m25p16CsPin, OWNER_FLASH, RESOURCE_SPI_CS, 0);
    IOConfigGPIO(m25p16CsPin, SPI_IO_CS_CFG);

    DISABLE_M25P16;

#ifndef M25P16_SPI_SHARED
    //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz
    spiSetDivisor(M25P16_SPI_INSTANCE, SPI_CLOCK_FAST);
#endif

    return m25p16_readIdentification();
}
Ejemplo n.º 26
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
	debug[3]++;
    mpuIntExtiInit();

    mpu6000AccAndGyroInit(lpf);

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); //high speed now that we don't need to write to the slow registers

    int16_t data[3];
    mpuGyroRead(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}
Ejemplo n.º 27
0
void rxSpiDeviceInit(rx_spi_type_e spiType)
{
    static bool hardwareInitialised = false;

    if (hardwareInitialised) {
        return;
    }

#ifdef USE_RX_SOFTSPI
    if (spiType == RX_SPI_SOFTSPI) {
        useSoftSPI = true;
        softSpiInit(&softSPIDevice);
    }
    const SPIDevice rxSPIDevice = SOFT_SPIDEV_1;
#else
    UNUSED(spiType);
    const SPIDevice rxSPIDevice = spiDeviceByInstance(RX_SPI_INSTANCE);
    IOInit(IOGetByTag(IO_TAG(RX_NSS_PIN)), OWNER_SPI, RESOURCE_SPI_CS, rxSPIDevice + 1);
#endif // USE_RX_SOFTSPI

#if defined(STM32F10X)
    RCC_AHBPeriphClockCmd(RX_NSS_GPIO_CLK_PERIPHERAL, ENABLE);
    RCC_AHBPeriphClockCmd(RX_CE_GPIO_CLK_PERIPHERAL, ENABLE);
#endif

#ifdef RX_CE_PIN
    // CE as OUTPUT
    IOInit(IOGetByTag(IO_TAG(RX_CE_PIN)), OWNER_RX_SPI, RESOURCE_RX_CE, rxSPIDevice + 1);
#if defined(STM32F10X)
    IOConfigGPIO(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG);
#elif defined(STM32F3) || defined(STM32F4)
    IOConfigGPIOAF(IOGetByTag(IO_TAG(RX_CE_PIN)), SPI_IO_CS_CFG, 0);
#endif
    RX_CE_LO();
#endif // RX_CE_PIN
    DISABLE_RX();

#ifdef RX_SPI_INSTANCE
    spiSetDivisor(RX_SPI_INSTANCE, SPI_CLOCK_STANDARD);
#endif
    hardwareInitialised = true;
}
Ejemplo n.º 28
0
//here we init only CS and try to init MAX for first time
void max7456Init(uint8_t video_system)
{
#ifdef MAX7456_SPI_CS_PIN
    max7456CsPin = IOGetByTag(IO_TAG(MAX7456_SPI_CS_PIN));
#endif
    IOInit(max7456CsPin, OWNER_OSD, RESOURCE_SPI_CS, 0);
    IOConfigGPIO(max7456CsPin, SPI_IO_CS_CFG);

    spiSetDivisor(MAX7456_SPI_INSTANCE, SPI_CLOCK_STANDARD);
    // force soft reset on Max7456
    ENABLE_MAX7456;
    max7456Send(VM0_REG, MAX7456_RESET);
    DISABLE_MAX7456;
    videoSignalCfg = video_system;

#ifdef MAX7456_DMA_CHANNEL_TX
    dmaSetHandler(MAX7456_DMA_IRQ_HANDLER_ID, max7456_dma_irq_handler, NVIC_PRIO_MAX7456_DMA, 0);
#endif
    //real init will be made letter when driver idle detect
}
Ejemplo n.º 29
0
bool mpu9250SpiDetect(void)
{
    uint8_t in;
    uint8_t attemptsRemaining = 20;

    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_SLOW_CLOCK); //low speed
    mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);

    do {
        delay(150);

        mpu9250ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
        if (in == MPU9250_WHO_AM_I_CONST) {
            break;
        }
        if (!attemptsRemaining) {
            return false;
        }
    } while (attemptsRemaining--);

    return true;
}
Ejemplo n.º 30
0
void mpu6000SpiGyroInit(uint8_t lpf)
{
    mpuIntExtiInit();

    mpu6000AccAndGyroInit();

    spiResetErrorCounter(MPU6000_SPI_INSTANCE);

    spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI);

    // Accel and Gyro DLPF Setting
    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
    delayMicroseconds(1);

    int16_t data[3];
    mpuGyroRead(data);

    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
        spiResetErrorCounter(MPU6000_SPI_INSTANCE);
        failureMode(FAILURE_GYRO_INIT_FAILED);
    }
}