Exemplos de _detect_version em C++ (Cpp)

Exemplo n.º 1

Arquivo: AP_Compass_HMC5843.cpp Projeto: Boyang--Li/ardupilot

bool AP_Compass_HMC5843::init()
{
    hal.scheduler->suspend_timer_procs();
    AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();

    if (!bus_sem || !bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        hal.console->printf("HMC5843: Unable to get bus semaphore\n");
        goto fail_sem;
    }

    if (!_bus->configure()) {
        hal.console->printf("HMC5843: Could not configure the bus\n");
        goto errout;
    }

    if (!_detect_version()) {
        hal.console->printf("HMC5843: Could not detect version\n");
        goto errout;
    }

    if (!_calibrate()) {
        hal.console->printf("HMC5843: Could not calibrate sensor\n");
        goto errout;
    }

    if (!_setup_sampling_mode()) {
        goto errout;
    }

    if (!_bus->start_measurements()) {
        hal.console->printf("HMC5843: Could not start measurements on bus\n");
        goto errout;
    }

    _initialised = true;

    bus_sem->give();
    hal.scheduler->resume_timer_procs();

    // perform an initial read
    read();

    _compass_instance = register_compass();
    set_dev_id(_compass_instance, _product_id);

    if (_force_external) {
        set_external(_compass_instance, true);
    }

    return true;

errout:
    bus_sem->give();

fail_sem:
    hal.scheduler->resume_timer_procs();

    return false;
}

Exemplo n.º 2

Arquivo: AP_Compass_HMC5843.cpp Projeto: ryankurte/ardupilot-public

// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
    uint8_t calibration_gain = 0x20;
    uint16_t expected_x = 715;
    uint16_t expected_yz = 715;
    float gain_multiple = 1.0;

    _bus_sem = _bus->get_semaphore();
    hal.scheduler->suspend_timer_procs();

    if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
        hal.console->printf_P(PSTR("HMC5843: Unable to get bus semaphore\n"));
        goto fail_sem;
    }

    if (!_bus->configure()) {
        hal.console->printf_P(PSTR("HMC5843: Could not configure the bus\n"));
        goto errout;
    }

    if (!_detect_version()) {
        hal.console->printf_P(PSTR("HMC5843: Could not detect version\n"));
        goto errout;
    }

    if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
        calibration_gain = 0x60;
        /*
          note that the HMC5883 datasheet gives the x and y expected
          values as 766 and the z as 713. Experiments have shown the x
          axis is around 766, and the y and z closer to 713.
         */
        expected_x = 766;
        expected_yz  = 713;
        gain_multiple = 660.0f / 1090;  // adjustment for runtime vs calibration gain
    }

    if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
        hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
        goto errout;
    }

    // leave test mode
    if (!re_initialise()) {
        goto errout;
    }

    if (!_bus->start_measurements()) {
        hal.console->printf_P(PSTR("HMC5843: Could not start measurements on bus\n"));
        goto errout;
    }
    _initialised = true;

    _bus_sem->give();
    hal.scheduler->resume_timer_procs();

    // perform an initial read
    read();

#if 0
    hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
                          _scaling[0], _scaling[1], _scaling[2]);
#endif

    _compass_instance = register_compass();
    set_dev_id(_compass_instance, _product_id);

    return true;

errout:
    _bus_sem->give();
fail_sem:
    hal.scheduler->resume_timer_procs();
    return false;
}