yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

UAVCAN Sensors: Improve error handling

Browse Source 
Also add new 'generic' device types for UAVCAN sensors

Other misc. cleanup and style changes.
This commit is contained in:
JacobCrabill
2020-03-12 11:58:40 -07:00
committed by Daniel Agar
parent 693a47fca3
commit 1e5784609f
7 changed files with 67 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/drv_sensor.h
View File

@@ -151,6 +151,18 @@
#define DRV_SENS_DEVTYPE_PCF8583 0x7e
#define DRV_TEL_DEVTYPE_BST 0x7f
// Generic types for unknown CAN sensors
#define DRV_ACC_DEVTYPE_UAVCAN 0x80
#define DRV_BARO_DEVTYPE_UAVCAN 0x81
#define DRV_BAT_DEVTYPE_UAVCAN 0x82
#define DRV_DIFF_PRESS_DEVTYPE_UAVCAN 0x83
#define DRV_FLOW_DEVTYPE_UAVCAN 0x84
#define DRV_GPS_DEVTYPE_UAVCAN 0x85
#define DRV_GYR_DEVTYPE_UAVCAN 0x86
#define DRV_IMU_DEVTYPE_UAVCAN 0x87
#define DRV_MAG_DEVTYPE_UAVCAN 0x88
#define DRV_DIST_DEVTYPE_UAVCAN 0x89
#define DRV_DEVTYPE_UNUSED 0xff
/*

27
src/drivers/uavcan/sensors/baro.cpp
View File

@@ -47,7 +47,8 @@ const char *const UavcanBarometerBridge::NAME = "baro";
#define UAVCAN_BARO_BASE_DEVICE_PATH "/dev/uavcan/baro"
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", UAVCAN_BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
UavcanCDevSensorBridgeBase("uavcan_baro", UAVCAN_BARO_BASE_DEVICE_PATH, UAVCAN_BARO_BASE_DEVICE_PATH,
ORB_ID(sensor_baro)),
_sub_air_pressure_data(node),
_sub_air_temperature_data(node)
{ }
@@ -98,10 +99,14 @@ UavcanBarometerBridge::air_pressure_sub_cb(const
}
// Cast our generic CDev pointer to the sensor-specific driver class
PX4Barometer *_baro = (PX4Barometer *)channel->h_driver;
PX4Barometer *baro = (PX4Barometer *)channel->h_driver;
_baro->set_temperature(last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS);
_baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar
if (baro == nullptr) {
return;
}
baro->set_temperature(last_temperature_kelvin + CONSTANTS_ABSOLUTE_NULL_CELSIUS);
baro->update(hrt_absolute_time(), msg.static_pressure / 100.0f); // Convert pressure to millibar
}
int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
@@ -109,8 +114,7 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
// update device id as we now know our device node_id
DeviceId device_id{_device_id};
// No sensor info is included int he StaticPressure msg; use some generic baro type
device_id.devid_s.devtype = DRV_BARO_DEVTYPE_MS5611;
device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN;
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
channel->h_driver = new PX4Barometer(device_id.devid, ORB_PRIO_HIGH);
@@ -119,9 +123,16 @@ int UavcanBarometerBridge::init_driver(uavcan_bridge::Channel *channel)
return PX4_ERROR;
}
PX4Barometer *_baro = (PX4Barometer *)channel->h_driver;
PX4Barometer *baro = (PX4Barometer *)channel->h_driver;
channel->class_instance = _baro->get_class_instance();
channel->class_instance = baro->get_class_instance();
if (channel->class_instance < 0) {
PX4_ERR("UavcanBaro: Unable to get a class instance");
delete baro;
channel->h_driver = nullptr;
return PX4_ERROR;
}
return PX4_OK;
}

2
src/drivers/uavcan/sensors/flow.cpp
View File

@@ -68,7 +68,7 @@ UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure<com::hex::equi
optical_flow_s flow{};
// We're only given an 8 bit field for sensor ID; just use the UAVCAN node ID
flow.sensor_id = (uint8_t)(msg.getSrcNodeID().get() & 0xFF);
flow.sensor_id = msg.getSrcNodeID().get();
flow.timestamp = hrt_absolute_time();
flow.integration_timespan = 1.e6f * msg.integration_interval; // s -> micros

38
src/drivers/uavcan/sensors/mag.cpp
View File

@@ -47,7 +47,7 @@ const char *const UavcanMagnetometerBridge::NAME = "mag";
#define UAVCAN_MAG_BASE_DEVICE_PATH "/dev/uavcan/mag"
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
UavcanCDevSensorBridgeBase("uavcan_mag", UAVCAN_MAG_BASE_DEVICE_PATH, UAVCAN_MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node),
_sub_mag2(node)
{
@@ -102,13 +102,17 @@ UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan:
}
// Cast our generic CDev pointer to the sensor-specific driver class
PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver;
PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver;
if (mag == nullptr) {
return;
}
const float x = msg.magnetic_field_ga[0];
const float y = msg.magnetic_field_ga[1];
const float z = msg.magnetic_field_ga[2];
_mag->update(hrt_absolute_time(), x, y, z);
mag->update(hrt_absolute_time(), x, y, z);
}
void
@@ -117,19 +121,23 @@ UavcanMagnetometerBridge::mag2_sub_cb(const
{
uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get());
if (channel == nullptr) {
if (channel == nullptr || channel->class_instance < 0) {
// Something went wrong - no channel to publish on; return
return;
}
// Cast our generic CDev pointer to the sensor-specific driver class
PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver;
PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver;
if (mag == nullptr) {
return;
}
const float x = msg.magnetic_field_ga[0];
const float y = msg.magnetic_field_ga[1];
const float z = msg.magnetic_field_ga[2];
_mag->update(hrt_absolute_time(), x, y, z);
mag->update(hrt_absolute_time(), x, y, z);
}
int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
@@ -137,8 +145,7 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
// update device id as we now know our device node_id
DeviceId device_id{_device_id};
// No sensor info is included in the MagneticFieldStrength msg; use some generic mag type
device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250;
device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN;
device_id.devid_s.address = static_cast<uint8_t>(channel->node_id);
channel->h_driver = new PX4Magnetometer(device_id.devid, ORB_PRIO_HIGH, ROTATION_NONE);
@@ -147,9 +154,18 @@ int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel)
return PX4_ERROR;
}
PX4Magnetometer *_mag = (PX4Magnetometer *)channel->h_driver;
_mag->set_external(true);
channel->class_instance = _mag->get_class_instance();
PX4Magnetometer *mag = (PX4Magnetometer *)channel->h_driver;
channel->class_instance = mag->get_class_instance();
if (channel->class_instance < 0) {
PX4_ERR("UavcanMag: Unable to get a class instance");
delete mag;
channel->h_driver = nullptr;
return PX4_ERROR;
}
mag->set_external(true);
return PX4_OK;
}

20
src/drivers/uavcan/sensors/sensor_bridge.cpp
View File

@@ -88,7 +88,7 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return; // Give up immediately - saves some CPU time
return; // Give up immediately - saves some CPU time
}
DEVICE_LOG("adding channel %d...", node_id);
@@ -111,9 +111,6 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// update device id as we now know our device node_id
_device_id.devid_s.address = static_cast<uint8_t>(node_id);
// initialize the driver, which registers the class device name and uORB publisher
// int ret = init_driver(channel);
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
@@ -124,11 +121,6 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
return;
}
// if (ret != PX4_OK) {
// DEVICE_LOG("INIT ERROR node %d errno %d", channel->node_id, ret);
// return;
// }
// Publish to the appropriate topic, abort on failure
channel->node_id = node_id;
channel->class_instance = class_instance;
@@ -137,17 +129,16 @@ UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH);
if (channel->orb_advert == nullptr) {
DEVICE_LOG("ADVERTISE FAILED");
DEVICE_LOG("uORB advertise failed. Out of instances?");
(void)unregister_class_devname(_class_devname, class_instance);
*channel = uavcan_bridge::Channel();
_out_of_channels = true;
return;
}
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->class_instance);
DEVICE_LOG("channel %d class instance %d ok", channel->node_id, channel->orb_instance);
}
//publish_sensor()
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -168,7 +159,8 @@ uavcan_bridge::Channel *UavcanCDevSensorBridgeBase::get_channel_for_node(int nod
// No such channel - try to create one
if (channel == nullptr) {
if (_out_of_channels) {
return channel; // Give up immediately - saves some CPU time
// We already determined we're out of class or uORB instances
return channel;
}
DEVICE_LOG("adding channel %d...", node_id);

2
src/lib/drivers/barometer/PX4Barometer.hpp
View File

@@ -57,7 +57,7 @@ public:
void update(hrt_abstime timestamp, float pressure);
int get_class_instance(void) { return _class_device_instance; };
int get_class_instance() { return _class_device_instance; };
void print_status();

2
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
View File

@@ -60,7 +60,7 @@ public:
void update(hrt_abstime timestamp_sample, float x, float y, float z);
int get_class_instance(void) { return _class_device_instance; };
int get_class_instance() { return _class_device_instance; };
void print_status();