fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 04:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master_uavcan_modular src/modules/uavcan/

Browse Source
This commit is contained in:
David Sidrane
2015-07-09 11:22:35 -10:00
parent 3fa7006576
commit 22d46fa733
13 changed files with 1258 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/sensors/gnss.cpp
+17 -11
View File
@@ -46,20 +46,21 @@
const char *const UavcanGnssBridge::NAME = "gnss";
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
_node(node),
_sub_fix(node),
_report_pub(-1)
_node(node),
_sub_fix(node),
_report_pub(-1)
{
}
int UavcanGnssBridge::init()
{
int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("GNSS fix sub failed %i", res);
return res;
}
return res;
}
@@ -71,8 +72,10 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() const
void UavcanGnssBridge::print_status() const
{
printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount());
if (_receiver_node_id < 0) {
printf("N/A\n");
} else {
printf("%d\n", _receiver_node_id);
}
@@ -84,6 +87,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
if (_receiver_node_id < 0) {
_receiver_node_id = msg.getSrcNodeID().get();
warnx("GNSS receiver node ID: %d", _receiver_node_id);
} else {
if (_receiver_node_id != msg.getSrcNodeID().get()) {
return; // This GNSS receiver is the redundant one, ignore it.
@@ -114,14 +118,15 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
// Vertical position uncertainty
report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
} else {
report.eph = -1.0F;
report.epv = -1.0F;
}
if (valid_velocity_covariance) {
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
float vel_cov[9];
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
/* There is a nonlinear relationship between the velocity vector and the heading.
@@ -139,9 +144,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
float vel_n_sq = vel_n * vel_n;
float vel_e_sq = vel_e * vel_e;
report.c_variance_rad =
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
(vel_e_sq * vel_cov[0] +
-2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric
vel_n_sq * vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
} else {
report.s_variance_m_s = -1.0F;
@@ -154,7 +159,8 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
report.vel_n_m_s = msg.ned_velocity[0];
report.vel_e_m_s = msg.ned_velocity[1];
report.vel_d_m_s = msg.ned_velocity[2];
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s);
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s *
report.vel_d_m_s);
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
report.vel_ned_valid = true;
src/modules/uavcan/sensors/mag.cpp
+35 -22
View File
@@ -41,9 +41,9 @@
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_BASE_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
@@ -55,15 +55,18 @@ _sub_mag(node)
int UavcanMagnetometerBridge::init()
{
int res = device::CDev::init();
if (res < 0) {
return res;
}
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
if (res < 0) {
log("failed to start uavcan sub: %d", res);
return res;
}
return 0;
}
@@ -74,6 +77,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
/* buffer must be large enough */
unsigned count = buflen / sizeof(struct mag_report);
if (count < 1) {
return -ENOSPC;
}
@@ -85,6 +89,7 @@ ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t b
last_read = _report.timestamp;
unlock();
return sizeof(struct mag_report);
} else {
/* no new data available, warn caller */
return -EAGAIN;
@@ -95,25 +100,31 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
{
switch (cmd) {
case SENSORIOCSQUEUEDEPTH: {
return OK; // Pretend that this stuff is supported to keep APM happy
}
return OK; // Pretend that this stuff is supported to keep APM happy
}
case MAGIOCSSCALE: {
std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
return 0;
}
std::memcpy(&_scale, reinterpret_cast<const void *>(arg), sizeof(_scale));
return 0;
}
case MAGIOCGSCALE: {
std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale));
return 0;
}
std::memcpy(reinterpret_cast<void *>(arg), &_scale, sizeof(_scale));
return 0;
}
case MAGIOCSELFTEST: {
return 0; // Nothing to do
}
return 0; // Nothing to do
}
case MAGIOCGEXTERNAL: {
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
}
case MAGIOCSSAMPLERATE: {
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
return 0; // Pretend that this stuff is supported to keep the sensor app happy
}
case MAGIOCCALIBRATE:
case MAGIOCGSAMPLERATE:
case MAGIOCSRANGE:
@@ -121,15 +132,17 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
case MAGIOCSLOWPASS:
case MAGIOCEXSTRAP:
case MAGIOCGLOWPASS: {
return -EINVAL;
}
return -EINVAL;
}
default: {
return CDev::ioctl(filp, cmd, arg);
}
return CDev::ioctl(filp, cmd, arg);
}
}
}
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
src/modules/uavcan/sensors/sensor_bridge.cpp
+8 -1
View File
@@ -45,7 +45,7 @@
/*
* IUavcanSensorBridge
*/
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list)
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
{
list.add(new UavcanBarometerBridge(node));
list.add(new UavcanMagnetometerBridge(node));
@@ -62,6 +62,7 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _channels;
}
@@ -107,6 +108,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
// Ask the CDev helper which class instance we can take
const int class_instance = register_class_devname(_class_devname);
if (class_instance < 0 || class_instance >= int(_max_channels)) {
_out_of_channels = true;
log("out of class instances");
@@ -119,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH);
if (channel->orb_advert < 0) {
log("ADVERTISE FAILED");
(void)unregister_class_devname(_class_devname, class_instance);
@@ -128,6 +131,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
log("channel %d class instance %d ok", channel->node_id, channel->class_instance);
}
assert(channel != nullptr);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
@@ -136,11 +140,13 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
{
unsigned out = 0;
for (unsigned i = 0; i < _max_channels; i++) {
if (_channels[i].node_id >= 0) {
out += 1;
}
}
return out;
}
@@ -152,6 +158,7 @@ void UavcanCDevSensorBridgeBase::print_status() const
if (_channels[i].node_id >= 0) {
printf("channel %d: node id %d --> class instance %d\n",
i, _channels[i].node_id, _channels[i].class_instance);
} else {
printf("channel %d: empty\n", i);
}
src/modules/uavcan/sensors/sensor_bridge.hpp
+10 -11
View File
@@ -45,7 +45,7 @@
/**
* A sensor bridge class must implement this interface.
*/
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*>
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
{
public:
static constexpr unsigned MAX_NAME_LEN = 20;
@@ -77,7 +77,7 @@ public:
* Sensor bridge factory.
* Creates all known sensor bridges and puts them in the linked list.
*/
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list);
static void make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list);
};
/**
@@ -86,8 +86,7 @@ public:
*/
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
{
struct Channel
{
struct Channel {
int node_id = -1;
orb_advert_t orb_advert = -1;
int class_instance = -1;
@@ -104,13 +103,13 @@ protected:
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
const orb_id_t orb_topic_sensor,
const unsigned max_channels = DEFAULT_MAX_CHANNELS) :
device::CDev(name, devname),
_max_channels(max_channels),
_class_devname(class_devname),
_orb_topic(orb_topic_sensor),
_channels(new Channel[max_channels])
{
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;