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;
}
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;
}
/* 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;
}
/* insert into empty tree */
static bool insert_first(struct CBTree *tree, void *obj)
{
tree->root = set_external(obj);
return true;
}