aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Compass: helper func

Browse Source
This commit is contained in:
Eugene Shamaev 2018-03-11 11:50:49 +02:00 committed by Tom Pittenger
parent 9f27a65f4b
commit 9195ba80fc
2 changed files with 50 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_Compass/AP_Compass_UAVCAN.cpp
View File

@ -39,7 +39,7 @@ extern const AP_HAL::HAL& hal;
AP_Compass_UAVCAN::AP_Compass_UAVCAN(Compass &compass):
AP_Compass_Backend(compass)
{
_mag_baro = hal.util->new_semaphore();
_sem_mag = hal.util->new_semaphore();
}
AP_Compass_UAVCAN::~AP_Compass_UAVCAN()
@ -47,15 +47,15 @@ AP_Compass_UAVCAN::~AP_Compass_UAVCAN()
if (!_initialized) {
return;
}
if (hal.can_mgr[_manager] == nullptr) {
return;
}
AP_UAVCAN *ap_uavcan = hal.can_mgr[_manager]->get_UAVCAN();
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(_manager);
if (ap_uavcan == nullptr) {
return;
}
ap_uavcan->remove_mag_listener(this);
delete _sem_mag;
debug_mag_uavcan(2, "AP_Compass_UAVCAN destructed\n\r");
}
@ -68,11 +68,8 @@ AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
AP_Compass_UAVCAN *sensor;
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] == nullptr) {
continue;
}
AP_UAVCAN *uavcan = hal.can_mgr[i]->get_UAVCAN();
if (uavcan == nullptr) {
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(i);
if (ap_uavcan == nullptr) {
continue;
}
uint8_t freemag = uavcan->find_smallest_free_mag_node();
@ -94,46 +91,45 @@ AP_Compass_Backend *AP_Compass_UAVCAN::probe(Compass &compass)
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;
}
}
AP_UAVCAN *ap_uavcan = AP_UAVCAN::get_uavcan(mgr);
if (ap_uavcan == nullptr) {
return;
}
_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;
}
@ -144,14 +140,14 @@ void AP_Compass_UAVCAN::read(void)
return;
}
if (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_sem_mag->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
_sum /= _count;
publish_filtered_field(_sum, _instance);
_sum.zero();
_count = 0;
_mag_baro->give();
_sem_mag->give();
}
}
@ -168,11 +164,11 @@ void AP_Compass_UAVCAN::handle_mag_msg(Vector3f &mag)
// correct raw_field for known errors
correct_field(raw_field, _instance);
if (_mag_baro->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
if (_sem_mag->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
// accumulate into averaging filter
_sum += raw_field;
_count++;
_mag_baro->give();
_sem_mag->give();
}
}

2
libraries/AP_Compass/AP_Compass_UAVCAN.h
View File

@ -28,5 +28,5 @@ private:
bool _initialized;
uint8_t _manager;
AP_HAL::Semaphore *_mag_baro;
AP_HAL::Semaphore *_sem_mag;
};