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;
}
Exemple #2
0
DexField* make_field_def(DexType* cls, const char* name, DexType* type,
    DexAccessFlags access = ACC_PUBLIC, bool external = false) {
  auto field = DexField::make_field(cls, DexString::make_string(name), type);
  if (external) {
    field->set_access(access);
    field->set_external();
  } else {
    field->make_concrete(access);
  }
  return field;
}
bool AP_Compass_PX4::init(void)
{
	_mag_fd[0] = open(MAG_BASE_DEVICE_PATH"0", O_RDONLY);
	_mag_fd[1] = open(MAG_BASE_DEVICE_PATH"1", O_RDONLY);
	_mag_fd[2] = open(MAG_BASE_DEVICE_PATH"2", O_RDONLY);

    _num_sensors = 0;
    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
        if (_mag_fd[i] >= 0) {
            _num_sensors = i+1;
        }
    }    
	if (_num_sensors == 0) {
        hal.console->printf("Unable to open " MAG_BASE_DEVICE_PATH"0" "\n");
        return false;
	}

    for (uint8_t i=0; i<_num_sensors; i++) {
        _instance[i] = register_compass();

        // get device id
        set_dev_id(_instance[i], ioctl(_mag_fd[i], DEVIOCGDEVICEID, 0));

        // average over up to 20 samples
        if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
            hal.console->printf("Failed to setup compass queue\n");
            return false;                
        }

        // remember if the compass is external
        set_external(_instance[i], ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
        _count[i] = 0;
        _sum[i].zero();

        set_milligauss_ratio(_instance[i], 1.0f);
    }

    // give the driver a chance to run, and gather one sample
    hal.scheduler->delay(40);
    accumulate();
    if (_count[0] == 0) {
        hal.console->printf("Failed initial compass accumulate\n");        
    }

    return true;
}
bool AP_Compass_UAVCAN::register_uavcan_compass(uint8_t mgr, uint8_t node)
{
    if (hal.can_mgr[mgr] != nullptr) {
        AP_UAVCAN *ap_uavcan = hal.can_mgr[mgr]->get_UAVCAN();
        if (ap_uavcan != nullptr) {
            _manager = mgr;

            if (ap_uavcan->register_mag_listener_to_node(this, node)) {
                _instance = register_compass();

                struct DeviceStructure {
                    uint8_t bus_type : 3;
                    uint8_t bus: 5;
                    uint8_t address;
                    uint8_t devtype;
                };
                union DeviceId {
                    struct DeviceStructure devid_s;
                    uint32_t devid;
                };
                union DeviceId d;

                d.devid_s.bus_type = 3;
                d.devid_s.bus = mgr;
                d.devid_s.address = node;
                d.devid_s.devtype = 1;

                set_dev_id(_instance, d.devid);
                set_external(_instance, true);

                _sum.zero();
                _count = 0;

                accumulate();

                debug_mag_uavcan(2, "AP_Compass_UAVCAN loaded\n\r");

                return true;
            }
        }
    }

    return false;
}
Exemple #5
0
/* insert into specific bit-position */
static bool insert_at(struct CBTree *tree, unsigned newbit, const void *key, unsigned klen, void *obj)
{
	/* location of current node/obj pointer under examination */
	struct Node **pos = &tree->root;
	struct Node *node;
	unsigned bit;

	while (is_node(*pos) && ((*pos)->bitpos < newbit)) {
		bit = get_bit((*pos)->bitpos, key, klen);
		pos = &(*pos)->child[bit];
	}

	bit = get_bit(newbit, key, klen);
	node = new_node(tree);
	if (!node)
		return false;
	node->bitpos = newbit;
	node->child[bit] = set_external(obj);
	node->child[bit ^ 1] = *pos;
	*pos = node;
	return true;
}
Exemple #6
0
/* insert into empty tree */
static bool insert_first(struct CBTree *tree, void *obj)
{
	tree->root = set_external(obj);
	return true;
}