예제 #1
0
static void hmc5883lConfigureDataReadyInterruptHandling(magDev_t* mag)
{
#ifdef USE_MAG_DATA_READY_SIGNAL
    if (mag->magIntExtiTag == IO_TAG_NONE) {
        return;
    }

    const IO_t magIntIO = IOGetByTag(mag->magIntExtiTag);

#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
    uint8_t status = IORead(magIntIO);
    if (!status) {
        return;
    }
#endif

#if defined (STM32F7)
    IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
    EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
    EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));
    EXTIEnable(magIntIO, true);
#else
    IOInit(magIntIO, OWNER_COMPASS_EXTI, 0);
    IOConfigGPIO(magIntIO, IOCFG_IN_FLOATING);
    EXTIHandlerInit(&mag->exti, hmc5883_extiHandler);
    EXTIConfig(magIntIO, &mag->exti, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
    EXTIEnable(magIntIO, true);
#endif
#else
    UNUSED(mag);
#endif
}
예제 #2
0
void mpuIntExtiInit(void)
{
    static bool mpuExtiInitDone = false;

    if (mpuExtiInitDone || !mpuIntExtiConfig) {
        return;
    }

#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)

    IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);

#ifdef ENSURE_MPU_DATA_READY_IS_LOW
    uint8_t status = IORead(mpuIntIO);
    if (status) {
        return;
    }
#endif

    IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
    IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING);   // TODO - maybe pullup / pulldown ?

    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
    EXTIEnable(mpuIntIO, true);
#endif

    mpuExtiInitDone = true; 
}
예제 #3
0
static void hmc5883lConfigureDataReadyInterruptHandling(void)
{
#ifdef USE_MAG_DATA_READY_SIGNAL

    if (!(hmc5883Config->intTag)) {
        return;
    }
    intIO = IOGetByTag(hmc5883Config->intTag);
#ifdef ENSURE_MAG_DATA_READY_IS_HIGH
    uint8_t status = IORead(intIO);
    if (!status) {
        return;
    }
#endif

    EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
    EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
    EXTIEnable(intIO, true);
#endif
}
예제 #4
0
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
{
    bool detected = false;

#ifdef STM32F10X
    // enable AFIO for EXTI support
    RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
#endif

#if defined(STM32F3) || defined(STM32F4)
    RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
#endif

    triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
    echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);

    if (IOGetOwner(triggerIO) != OWNER_FREE) {
        return false;
    }

    if (IOGetOwner(echoIO) != OWNER_FREE) {
        return false;
    }

    // trigger pin
    IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
    IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
    IOLo(triggerIO);
    delay(100);

    // echo pin
    IOInit(echoIO, OWNER_SONAR_ECHO, 0);
    IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);

    // HC-SR04 echo line should be low by default and should return a response pulse when triggered
    if (IORead(echoIO) == false) {
        for (int i = 0; i < 5 && !detected; i++) {
            timeMs_t requestTime = millis();
            hcsr04_start_reading();

            while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
                if (IORead(echoIO) == true) {
                    detected = true;
                    break;
                }
            }
        }
    }

    if (detected) {
        // Hardware detected - configure the driver
#ifdef USE_EXTI
        EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
        EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
        EXTIEnable(echoIO, true);
#endif

        dev->delayMs = 100;
        dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
        dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
        dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;

        dev->init = &hcsr04_init;
        dev->update = &hcsr04_update;
        dev->read = &hcsr04_get_distance;

        return true;
    }
    else {
        // Not detected - free resources
        IORelease(triggerIO);
        IORelease(echoIO);
        return false;
    }
}
예제 #5
0
void hmc5883lInit(void)
{
    int16_t magADC[3];
    int i;
    int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
    bool bret = true;           // Error indicator

#ifdef USE_MAG_DATA_READY_SIGNAL
    if (hmc5883Config && hmc5883Config->io) {
        intIO = IOGetByTag(hmc5883Config->io);
        IOInit(intIO, OWNER_SYSTEM, RESOURCE_INPUT | RESOURCE_EXTI);
        IOConfigGPIO(intIO, IOCFG_IN_FLOATING);
    }
#endif
    delay(50);
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
    // Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
    // The new gain setting is effective from the second measurement and on.
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x60); // Set the Gain to 2.5Ga (7:5->011)
    delay(100);
    hmc5883lRead(magADC);

    for (i = 0; i < 10; i++) {  // Collect 10 samples
        i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
        delay(50);
        hmc5883lRead(magADC);       // Get the raw values in case the scales have already been changed.

        // Since the measurements are noisy, they should be averaged rather than taking the max.
        xyz_total[X] += magADC[X];
        xyz_total[Y] += magADC[Y];
        xyz_total[Z] += magADC[Z];

        // Detect saturation.
        if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
            bret = false;
            break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
        }
        LED1_TOGGLE;
    }

    // Apply the negative bias. (Same gain)
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
    for (i = 0; i < 10; i++) {
        i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
        delay(50);
        hmc5883lRead(magADC);               // Get the raw values in case the scales have already been changed.

        // Since the measurements are noisy, they should be averaged.
        xyz_total[X] -= magADC[X];
        xyz_total[Y] -= magADC[Y];
        xyz_total[Z] -= magADC[Z];

        // Detect saturation.
        if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
            bret = false;
            break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
        }
        LED1_TOGGLE;
    }

    magGain[X] = fabsf(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
    magGain[Y] = fabsf(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
    magGain[Z] = fabsf(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);

    // leave test mode
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x70);   // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFB, 0x20);   // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
    i2cWrite(HMC5883L_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 0x00);    // Mode register             -- 000000 00    continuous Conversion Mode
    delay(100);

    if (!bret) {                // Something went wrong so get a best guess
        magGain[X] = 1.0f;
        magGain[Y] = 1.0f;
        magGain[Z] = 1.0f;
    }
    
#ifdef USE_MAG_DATA_READY_SIGNAL
    do {
        if (!(hmc5883Config && intIO))
            break;
# ifdef ENSURE_MAG_DATA_READY_IS_HIGH
        if (!IORead(intIO))
            break;
# endif
        EXTIHandlerInit(&hmc5883_extiCallbackRec, hmc5883_extiHandler);
        EXTIConfig(intIO, &hmc5883_extiCallbackRec, NVIC_PRIO_MAG_INT_EXTI, EXTI_Trigger_Rising);
        EXTIEnable(intIO, true);
    } while (0);
#endif
}