dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Upgrade UAVCAN to multi pub/sub A API

This commit is contained in:
Lorenz Meier 2015-01-25 21:56:56 +01:00
parent 95462c5b7c
commit 1cc4c808a8
10 changed files with 17 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
makefiles/config_px4fmu-v2_default.mk
View File

@ -70,7 +70,7 @@ MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
#MODULES += modules/uavcan
MODULES += modules/uavcan
MODULES += modules/land_detector
#

2
src/modules/uavcan/module.mk
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
#
# Redistribution and use in source and binary forms, with or without

9
src/modules/uavcan/sensors/baro.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -38,15 +38,10 @@
#include "baro.hpp"
#include <cmath>
static const orb_id_t BARO_TOPICS[2] = {
ORB_ID(sensor_baro0),
ORB_ID(sensor_baro1)
};
const char *const UavcanBarometerBridge::NAME = "baro";
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS),
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)),
_sub_air_data(node)
{
}

2
src/modules/uavcan/sensors/baro.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

2
src/modules/uavcan/sensors/gnss.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

2
src/modules/uavcan/sensors/gnss.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

10
src/modules/uavcan/sensors/mag.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -39,16 +39,10 @@
#include <systemlib/err.h>
static const orb_id_t MAG_TOPICS[3] = {
ORB_ID(sensor_mag0),
ORB_ID(sensor_mag1),
ORB_ID(sensor_mag2)
};
const char *const UavcanMagnetometerBridge::NAME = "mag";
UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)),
_sub_mag(node)
{
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;

2
src/modules/uavcan/sensors/mag.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

6
src/modules/uavcan/sensors/sensor_bridge.cpp
View File

@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
}
}
delete [] _orb_topics;
delete [] _channels;
}
@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
}
// Publish to the appropriate topic, abort on failure
channel->orb_id = _orb_topics[class_instance];
channel->node_id = node_id;
channel->class_instance = class_instance;
channel->orb_advert = orb_advertise(channel->orb_id, report);
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);
@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
}
assert(channel != nullptr);
(void)orb_publish(channel->orb_id, channel->orb_advert, report);
(void)orb_publish(_orb_topic, channel->orb_advert, report);
}
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const

11
src/modules/uavcan/sensors/sensor_bridge.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -90,28 +90,27 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
struct Channel
{
int node_id = -1;
orb_id_t orb_id = nullptr;
orb_advert_t orb_advert = -1;
int class_instance = -1;
int orb_instance = -1;
};
const unsigned _max_channels;
const char *const _class_devname;
orb_id_t *const _orb_topics;
const orb_id_t _orb_topic;
Channel *const _channels;
bool _out_of_channels = false;
protected:
template <unsigned MaxChannels>
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
const orb_id_t (&orb_topics)[MaxChannels]) :
const orb_id_t orb_topic_sensor) :
device::CDev(name, devname),
_max_channels(MaxChannels),
_class_devname(class_devname),
_orb_topics(new orb_id_t[MaxChannels]),
_orb_topic(orb_topic_sensor),
_channels(new Channel[MaxChannels])
{
memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
_device_id.devid_s.bus = 0;
}