Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Merged beta into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-22 14:06:01 +02:00
parent 0d8de2b84d f371ece43b
commit 21d1b4ba2e
25 changed files with 1385 additions and 238 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol → ROMFS/px4fmu_common/init.d/INACTIVE_13001_caipirinha_vtol
View File
ROMFS/px4fmu_common/init.d/13002_firefly6 → ROMFS/px4fmu_common/init.d/INACTIVE_13002_firefly6
View File
ROMFS/px4fmu_common/init.d/13003_quad_tailsitter → ROMFS/px4fmu_common/init.d/INACTIVE_13003_quad_tailsitter
View File
ROMFS/px4fmu_common/init.d/13004_quad+_tailsitter → ROMFS/px4fmu_common/init.d/INACTIVE_13004_quad+_tailsitter
View File
ROMFS/px4fmu_common/init.d/INACTIVE_13005_vtol_AAERT_quad
+50
View File
@@ -0,0 +1,50 @@
#!nsh
#
# @name Generic AAERT tailplane airframe with Quad VTOL.
#
# @type Standard VTOL
#
# @maintainer Simon Wilks <simon@uaventure.com>
#
sh /etc/init.d/rc.vtol_defaults
if [ $AUTOCNF == yes ]
then
param set VT_TYPE 2
param set VT_MOT_COUNT 4
param set VT_TRANS_THR 0.75
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.15
param set MC_ROLLRATE_I 0.002
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_FF 0.0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.12
param set MC_PITCHRATE_I 0.002
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_FF 0.0
param set MC_YAW_P 2.8
param set MC_YAW_FF 0.5
param set MC_YAWRATE_P 0.22
param set MC_YAWRATE_I 0.02
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_FF 0.0
fi
set MIXER vtol_quad_x
set PWM_OUT 12345678
set MIXER_AUX vtol_AAERT
set PWM_AUX_RATE 50
set PWM_AUX_OUT 1234
set PWM_AUX_DISARMED 1000
set PWM_AUX_MIN 1000
set PWM_AUX_MAX 2000
set MAV_TYPE 22
param set VT_MOT_COUNT 4
param set VT_IDLE_PWM_MC 1080
param set VT_TYPE 2
ROMFS/px4fmu_common/init.d/rc.uavcan
+37 -3
View File
@@ -3,16 +3,50 @@
# UAVCAN initialization script.
#
#
# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
# TODO there should be a smarter way.
#
set UAVCAN_ENABLE 0
if param compare UAVCAN_ENABLE 1
then
set UAVCAN_ENABLE 1
fi
if param compare UAVCAN_ENABLE 2
then
set UAVCAN_ENABLE 2
fi
echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
#
# Starting stuff according to UAVCAN_ENABLE value
#
if [ $UAVCAN_ENABLE -ge 1 ]
then
if uavcan start
then
# First sensor publisher to initialize takes lowest instance ID
# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
sleep 1
echo "[i] UAVCAN started"
else
echo "[i] ERROR: Could not start UAVCAN"
tone_alarm $TUNE_ERR
fi
fi
if [ $UAVCAN_ENABLE -ge 2 ]
then
if uavcan start fw
then
echo "[i] UAVCAN servers started"
else
echo "[i] ERROR: Could not start UAVCAN servers"
tone_alarm $TUNE_ERR
fi
fi
if [ $UAVCAN_ENABLE -ge 1 ]
then
# First sensor publisher to initialize takes lowest instance ID
# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
sleep 8
fi
ROMFS/px4fmu_common/init.d/rcS
+5 -4
View File
@@ -300,6 +300,11 @@ then
then
fi
#
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Sensors System (start before Commander so Preflight checks are properly run)
#
@@ -505,10 +510,6 @@ then
fi
fi
# UAVCAN
#
sh /etc/init.d/rc.uavcan
#
# Logging
#
Tools/check_submodules.sh
+6 -12
View File
@@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ];
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
@@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ];
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
@@ -70,8 +68,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d src/lib/eigen ]
@@ -92,8 +89,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d Tools/gencpp ]
@@ -114,8 +110,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
if [ -d Tools/genmsg ]
@@ -136,8 +131,7 @@ then
exit 1
fi
else
git submodule init;
git submodule update;
git submodule update --init --recursive
fi
exit 0
src/lib/uavcan
+1 -1
Submodule src/lib/uavcan updated: 988e404586...3ae5400aa5
src/modules/uavcan/actuators/esc.cpp
+21 -13
View File
@@ -68,8 +68,8 @@ int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
if (res < 0) {
warnx("ESC status sub failed %i", res);
return res;
}
@@ -83,9 +83,9 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
if ((outputs == nullptr) ||
(num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -94,9 +94,11 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
* Rate limiting - we don't want to congest the bus
*/
const auto timestamp = _node.getMonotonicTime();
if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
return;
}
_prev_cmd_pub = timestamp;
/*
@@ -110,15 +112,17 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
for (unsigned i = 0; i < num_outputs; i++) {
if (_armed_mask & MOTOR_BIT(i)) {
float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
// trim negative values back to 0. Previously
// we set this to 0.1, which meant motors kept
// spinning when armed, but that should be a
// policy decision for a specific vehicle
// type, as it is not appropriate for all
// types of vehicles (eg. fixed wing).
if (scaled < 0.0F) {
scaled = 0.0F;
}
}
if (scaled > cmd_max) {
scaled = cmd_max;
perf_count(_perfcnt_scaling_error);
@@ -127,6 +131,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
msg.cmd.push_back(static_cast<int>(scaled));
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
} else {
msg.cmd.push_back(static_cast<unsigned>(0));
}
@@ -143,6 +148,7 @@ void UavcanEscController::arm_all_escs(bool arm)
{
if (arm) {
_armed_mask = -1;
} else {
_armed_mask = 0;
}
@@ -152,6 +158,7 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
{
if (arm) {
_armed_mask = MOTOR_BIT(num);
} else {
_armed_mask = 0;
}
@@ -176,13 +183,14 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
{
_esc_status.counter += 1;
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub != nullptr) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
src/modules/uavcan/actuators/esc.hpp
+4 -4
View File
@@ -54,7 +54,7 @@
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
UavcanEscController(uavcan::INode &node);
~UavcanEscController();
int init();
@@ -79,12 +79,12 @@ private:
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
typedef uavcan::MethodBinder<UavcanEscController *,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};
src/modules/uavcan/module.mk
+6 -1
View File
@@ -44,6 +44,7 @@ WFRAME_LARGER_THAN = 1400
# Main
SRCS += uavcan_main.cpp \
uavcan_servers.cpp \
uavcan_clock.cpp \
uavcan_params.c
@@ -65,7 +66,11 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC))
INCLUDE_DIRS += $(LIBUAVCAN_INC)
# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
override EXTRADEFINES := $(EXTRADEFINES) \
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03 \
-DUAVCAN_NO_ASSERTIONS \
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48 \
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
#
# libuavcan drivers for STM32
src/modules/uavcan/sensors/baro.cpp
+3 -5
View File
@@ -45,9 +45,7 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
_sub_air_pressure_data(node),
_sub_air_temperature_data(node),
_reports(nullptr)
{
last_temperature = 0.0f;
}
{ }
int UavcanBarometerBridge::init()
{
@@ -153,7 +151,7 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
void UavcanBarometerBridge::air_temperature_sub_cb(const
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
{
last_temperature = msg.static_temperature;
last_temperature_kelvin = msg.static_temperature;
}
void UavcanBarometerBridge::air_pressure_sub_cb(const
@@ -162,7 +160,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
baro_report report;
report.timestamp = msg.getMonotonicTimestamp().toUSec();
report.temperature = last_temperature;
report.temperature = last_temperature_kelvin - 273.15F;
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
report.error_count = 0;
src/modules/uavcan/sensors/baro.hpp
+1 -1
View File
@@ -78,6 +78,6 @@ private:
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
unsigned _msl_pressure = 101325;
ringbuffer::RingBuffer *_reports;
float last_temperature;
float last_temperature_kelvin = 0.0f;
};
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(nullptr)
_node(node),
_sub_fix(node),
_report_pub(nullptr)
{
}
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
+39 -26
View File
@@ -41,11 +41,11 @@
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;
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
_scale.x_scale = 1.0F;
_scale.y_scale = 1.0F;
@@ -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) {
DEVICE_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,23 +132,25 @@ 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::MagneticFieldStrength>
&msg)
{
lock();
_report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything
_report.timestamp = msg.getMonotonicTimestamp().toUSec();
_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
_report.z = (msg.magnetic_field_ga[2] - _scale.z_offset) * _scale.z_scale;
unlock();
publish(msg.getSrcNodeID().get(), &_report);
src/modules/uavcan/sensors/mag.hpp
+4 -4
View File
@@ -40,7 +40,7 @@
#include "sensor_bridge.hpp"
#include <drivers/drv_mag.h>
#include <uavcan/equipment/ahrs/Magnetometer.hpp>
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
{
@@ -57,14 +57,14 @@ private:
ssize_t read(struct file *filp, char *buffer, size_t buflen);
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &msg);
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
void (UavcanMagnetometerBridge::*)
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &) >
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
MagCbBinder;
uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
mag_scale _scale = {};
mag_report _report = {};
};
src/modules/uavcan/sensors/sensor_bridge.cpp
+7 -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;
DEVICE_LOG("out of class instances");
@@ -128,6 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
DEVICE_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 +139,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 +157,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 = nullptr;
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;
src/modules/uavcan/uavcan_main.cpp
+211 -113
View File
@@ -53,15 +53,12 @@
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
#include <uavcan/util/templates.hpp>
#if defined(USE_FW_NODE_SERVER)
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
#endif
/**
* @file uavcan_main.cpp
@@ -76,33 +73,28 @@
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
#if defined(USE_FW_NODE_SERVER)
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
uavcan_posix::FirmwareVersionChecker fw_version_checker;
#endif
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_node_mutex(),
#if !defined(USE_FW_NODE_SERVER)
_esc_controller(_node)
#else
_esc_controller(_node),
_fileserver_backend(_node),
_node_info_retriever(_node),
_fw_upgrade_trigger(_node, fw_version_checker),
_fw_server(_node, _fileserver_backend)
#endif
{
_task_should_exit = false;
_fw_server_action = None;
_fw_server_status = -1;
_tx_injector = nullptr;
_control_topics[0] = ORB_ID(actuator_controls_0);
_control_topics[1] = ORB_ID(actuator_controls_1);
_control_topics[2] = ORB_ID(actuator_controls_2);
_control_topics[3] = ORB_ID(actuator_controls_3);
const int res = pthread_mutex_init(&_node_mutex, nullptr);
int res = pthread_mutex_init(&_node_mutex, nullptr);
if (res < 0) {
std::abort();
}
res = sem_init(&_server_command_sem, 0 , 0);
if (res < 0) {
std::abort();
@@ -123,7 +115,11 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
UavcanNode::~UavcanNode()
{
fw_server(Stop);
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
@@ -161,13 +157,127 @@ UavcanNode::~UavcanNode()
perf_free(_perfcnt_node_spin_elapsed);
perf_free(_perfcnt_esc_mixer_output_elapsed);
perf_free(_perfcnt_esc_mixer_total_elapsed);
#if defined(USE_FW_NODE_SERVER)
delete(_server_instance);
#endif
pthread_mutex_destroy(&_node_mutex);
sem_destroy(&_server_command_sem);
}
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
{
int rv = -1;
if (UavcanNode::instance()) {
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
rv = 0;
}
return rv;
}
int UavcanNode::start_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers == nullptr) {
rv = UavcanServers::start(2, _node);
if (rv >= 0) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().installRxFrameListener(driver.get());
* would require a dynamic cast and rtti is not enabled.
*/
UavcanServers::instance()->attachITxQueueInjector(&_tx_injector);
}
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::request_fw_check()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
_servers->requestCheckAllNodesFirmwareAndUpdate();
rv = 0;
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::stop_fw_server()
{
int rv = -1;
_fw_server_action = Busy;
UavcanServers *_servers = UavcanServers::instance();
if (_servers != nullptr) {
/*
* Set our pointer to to the injector
* This is a work around as
* main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
_tx_injector = nullptr;
rv = _servers->stop();
}
_fw_server_action = None;
sem_post(&_server_command_sem);
return rv;
}
int UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action == None) {
_fw_server_action = action;
sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
@@ -213,6 +323,11 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
return -1;
}
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
@@ -248,7 +363,7 @@ void UavcanNode::fill_node_info()
assert(fw_git_short[8] == '\0');
char *end = nullptr;
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
@@ -256,21 +371,7 @@ void UavcanNode::fill_node_info()
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
hwver.major = 1;
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
hwver.major = 2;
} else {
; // All other values of HW_ARCH resolve to zero
}
uint8_t udid[12] = {}; // Someone seems to love magic numbers
get_board_serial(udid);
uavcan::copy(udid, udid + sizeof(udid), hwver.unique_id.begin());
getHardwareVersion(hwver);
_node.setHardwareVersion(hwver);
}
@@ -315,75 +416,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
br = br->getSibling();
}
#if defined(USE_FW_NODE_SERVER)
/* Initialize the fw version checker.
* giving it it's path
*/
ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
return ret;
}
/* Create dynamic node id server for the Firmware updates directory */
_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
if (_server_instance == 0) {
return -ENOMEM;
}
/* Initialize the dynamic node id server */
ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
if (ret < 0) {
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
if (ret < 0) {
return ret;
}
#endif
/* Start the Node */
return _node.start();
@@ -398,6 +431,11 @@ void UavcanNode::node_spin_once()
warnx("node spin error %i", spin_res);
}
if (_tx_injector != nullptr) {
_tx_injector->injectTxFramesInto(_node);
}
perf_end(_perfcnt_node_spin_elapsed);
}
@@ -446,7 +484,7 @@ int UavcanNode::run()
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
_node.setModeOperational();
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
@@ -466,6 +504,24 @@ int UavcanNode::run()
while (!_task_should_exit) {
switch (_fw_server_action) {
case Start:
_fw_server_status = start_fw_server();
break;
case Stop:
_fw_server_status = stop_fw_server();
break;
case CheckFW:
_fw_server_status = request_fw_check();
break;
case None:
default:
break;
}
// update actuator controls subscriptions if needed
if (_groups_subscribed != _groups_required) {
subscribe();
@@ -622,6 +678,8 @@ UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t co
int
UavcanNode::teardown()
{
sem_post(&_server_command_sem);
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
@@ -812,7 +870,7 @@ UavcanNode::print_info()
static void print_usage()
{
warnx("usage: \n"
"\tuavcan {start|status|stop|arm|disarm}");
"\tuavcan {start [fw]|status|stop [all|fw]|arm|disarm|update fw}");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
@@ -824,8 +882,21 @@ int uavcan_main(int argc, char *argv[])
::exit(1);
}
bool fw = argc > 2 && !std::strcmp(argv[2], "fw");
if (!std::strcmp(argv[1], "start")) {
if (UavcanNode::instance()) {
if (fw && UavcanServers::instance() == nullptr) {
int rv = UavcanNode::instance()->fw_server(UavcanNode::Start);
if (rv < 0) {
warnx("Firmware Server Failed to Start %d", rv);
::exit(rv);
}
::exit(0);
}
// Already running, no error
warnx("already started");
::exit(0);
@@ -856,6 +927,20 @@ int uavcan_main(int argc, char *argv[])
errx(1, "application not running");
}
if (fw && !std::strcmp(argv[1], "update")) {
if (UavcanServers::instance() == nullptr) {
errx(1, "firmware server is not running");
}
int rv = UavcanNode::instance()->fw_server(UavcanNode::CheckFW);
::exit(rv);
}
if (fw && (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info"))) {
printf("Firmware Server is %s\n", UavcanServers::instance() ? "Running" : "Stopped");
::exit(0);
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
::exit(0);
@@ -872,8 +957,21 @@ int uavcan_main(int argc, char *argv[])
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
::exit(0);
if (fw) {
int rv = inst->fw_server(UavcanNode::Stop);
if (rv < 0) {
warnx("Firmware Server Failed to Stop %d", rv);
::exit(rv);
}
::exit(0);
} else {
delete inst;
::exit(0);
}
}
print_usage();
src/modules/uavcan/uavcan_main.hpp
+14 -26
View File
@@ -48,13 +48,7 @@
#include "actuators/esc.hpp"
#include "sensors/sensor_bridge.hpp"
#if defined(USE_FW_NODE_SERVER)
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
#endif
# include "uavcan_servers.hpp"
/**
* @file uavcan_main.hpp
@@ -65,10 +59,6 @@
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH PX4_ROOTFSDIR"/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH PX4_ROOTFSDIR"/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
// we add two to allow for actuator_direct and busevent
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
@@ -84,8 +74,7 @@ class UavcanNode : public device::CDev
static constexpr unsigned PollTimeoutMs = 10;
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
/*
* This memory is reserved for uavcan to use for queuing CAN frames.
* At 1Mbit there is approximately one CAN frame every 145 uS.
@@ -99,11 +88,12 @@ class UavcanNode : public device::CDev
*/
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
static constexpr unsigned StackSize = 3400;
static constexpr unsigned StackSize = 1600;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
enum eServerAction {None, Start, Stop, CheckFW , Busy};
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
@@ -126,6 +116,9 @@ public:
void print_info();
static UavcanNode *instance() { return _instance; }
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
int fw_server(eServerAction action);
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
private:
void fill_node_info();
@@ -133,10 +126,14 @@ private:
void node_spin_once();
int run();
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
int start_fw_server();
int stop_fw_server();
int request_fw_check();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
volatile eServerAction _fw_server_action;
int _fw_server_status;
int _armed_sub = -1; ///< uORB subscription of the arming status
actuator_armed_s _armed = {}; ///< the arming request of the system
bool _is_armed = false; ///< the arming status of the actuators on the bus
@@ -151,22 +148,13 @@ private:
Node _node; ///< library instance
pthread_mutex_t _node_mutex;
sem_t _server_command_sem;
UavcanEscController _esc_controller;
#if defined(USE_FW_NODE_SERVER)
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
#endif
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
MixerGroup *_mixers = nullptr;
ITxQueueInjector *_tx_injector;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
src/modules/uavcan/uavcan_params.c
+5 -2
View File
@@ -41,10 +41,13 @@
/**
* Enable UAVCAN.
*
* Enables support for UAVCAN-interfaced actuators and sensors.
* Allowed values:
* 0 - UAVCAN disabled.
* 1 - Enabled support for UAVCAN actuators and sensors.
* 2 - Enabled support for dynamic node ID allocation and firmware update.
*
* @min 0
* @max 1
* @max 2
* @group UAVCAN
*/
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
src/modules/uavcan/uavcan_servers.cpp
+293
View File
@@ -0,0 +1,293 @@
/****************************************************************************
*
* Copyright (c) 2014 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <nuttx/config.h>
#include <cstdlib>
#include <cstring>
#include <fcntl.h>
#include <memory>
#include <pthread.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/board_serial.h>
#include <systemlib/scheduling_priorities.h>
#include <systemlib/git_version.h>
#include <version/version.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include "uavcan_main.hpp"
#include "uavcan_servers.hpp"
#include "uavcan_virtual_can_driver.hpp"
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
//todo:The Inclusion of file_server_backend is killing
// #include <sys/types.h> and leaving OK undefined
# define OK 0
/**
* @file uavcan_servers.cpp
*
* Implements basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* David Sidrane <david_s5@nscdg.com>
*/
/*
* UavcanNode
*/
UavcanServers *UavcanServers::_instance;
UavcanServers::UavcanServers(uavcan::INode &main_node) :
_subnode_thread(-1),
_vdriver(UAVCAN_STM32_NUM_IFACES, uavcan_stm32::SystemClock::instance()),
_subnode(_vdriver, uavcan_stm32::SystemClock::instance()),
_main_node(main_node),
_tracer(),
_storage_backend(),
_fw_version_checker(),
_server_instance(_subnode, _storage_backend, _tracer),
_fileserver_backend(_subnode),
_node_info_retriever(_subnode),
_fw_upgrade_trigger(_subnode, _fw_version_checker),
_fw_server(_subnode, _fileserver_backend),
_mutex_inited(false),
_check_fw(false)
{
}
UavcanServers::~UavcanServers()
{
if (_mutex_inited) {
(void)Lock::deinit(_subnode_mutex);
}
_main_node.getDispatcher().removeRxFrameListener();
}
int UavcanServers::stop(void)
{
UavcanServers *server = instance();
if (server == nullptr) {
warnx("Already stopped");
return -1;
}
_instance = nullptr;
if (server->_subnode_thread != -1) {
pthread_cancel(server->_subnode_thread);
pthread_join(server->_subnode_thread, NULL);
}
server->_main_node.getDispatcher().removeRxFrameListener();
delete server;
return 0;
}
int UavcanServers::start(unsigned num_ifaces, uavcan::INode &main_node)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* Node init
*/
_instance = new UavcanServers(main_node);
if (_instance == nullptr) {
warnx("Out of memory");
return -2;
}
int rv = _instance->init(num_ifaces);
if (rv < 0) {
warnx("Node init failed: %d", rv);
delete _instance;
_instance = nullptr;
return rv;
}
/*
* Start the thread. Normally it should never exit.
*/
pthread_attr_t tattr;
struct sched_param param;
pthread_attr_init(&tattr);
tattr.stacksize = StackSize;
param.sched_priority = Priority;
pthread_attr_setschedparam(&tattr, &param);
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
rv = pthread_create(&_instance->_subnode_thread, &tattr, static_cast<pthread_startroutine_t>(run_trampoline), NULL);
if (rv < 0) {
warnx("pthread_create() failed: %d", errno);
rv = -errno;
delete _instance;
_instance = nullptr;
}
return rv;
}
int UavcanServers::init(unsigned num_ifaces)
{
errno = 0;
/*
* Initialize the mutex.
* giving it its path
*/
int ret = Lock::init(_subnode_mutex);
if (ret < 0) {
warnx("Lock init: %d", errno);
return ret;
}
_mutex_inited = true;
_subnode.setNodeID(_main_node.getNodeID());
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
/*
* Initialize the fw version checker.
* giving it its path
*/
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
if (ret < 0) {
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
return ret;
}
/* Start fw file server back */
ret = _fw_server.start();
if (ret < 0) {
warnx("BasicFileServer init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
if (ret < 0) {
warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
return ret;
}
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
ret = _tracer.init(UAVCAN_LOG_FILE);
if (ret < 0) {
warnx("FileEventTracer init: %d, errno: %d", ret, errno);
return ret;
}
/* hardware version */
uavcan::protocol::HardwareVersion hwver;
UavcanNode::getHardwareVersion(hwver);
/* Initialize the dynamic node id server */
ret = _server_instance.init(hwver.unique_id);
if (ret < 0) {
warnx("CentralizedServer init: %d", ret);
return ret;
}
/* Start node info retriever to fetch node info from new nodes */
ret = _node_info_retriever.start();
if (ret < 0) {
warnx("NodeInfoRetriever init: %d", ret);
return ret;
}
/* Start the fw version checker */
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
if (ret < 0) {
warnx("FirmwareUpdateTrigger init: %d", ret);
return ret;
}
/* Start the Node */
return OK;
}
__attribute__((optimize("-O0")))
pthread_addr_t UavcanServers::run(pthread_addr_t)
{
Lock lock(_subnode_mutex);
while (1) {
if (_check_fw == true) {
_check_fw = false;
_node_info_retriever.invalidateAll();
}
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100));
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
warnx("exiting.");
return (pthread_addr_t) 0;
}
src/modules/uavcan/uavcan_servers.hpp
+145
View File
@@ -0,0 +1,145 @@
/****************************************************************************
*
* Copyright (c) 2014 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
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <nuttx/config.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/device/device.h>
#include <systemlib/perf_counter.h>
#include <uavcan/node/sub_node.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
# include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
# include <uavcan/protocol/node_info_retriever.hpp>
# include <uavcan_posix/basic_file_server_backend.hpp>
# include <uavcan/protocol/firmware_update_trigger.hpp>
# include <uavcan/protocol/file_server.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
# include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
# include <uavcan_posix/firmware_version_checker.hpp>
# include "uavcan_virtual_can_driver.hpp"
/**
* @file uavcan_servers.hpp
*
* Defines basic functionality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author David Sidrane <david_s5@nscdg.com>
*/
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
/**
* A UAVCAN Server Sub node.
*/
class UavcanServers
{
static constexpr unsigned MemPoolSize = 64 * uavcan::MemPoolBlockSize;
static constexpr unsigned MaxCanFramsPerTransfer = 63;
/**
* This number is based on the worst case max number of frames per interface. With
* MemPoolBlockSize set at 48 this is 6048 Bytes.
*
* The servers can be forced to use the primary interface only, this can be achieved simply by passing
* 1 instead of UAVCAN_STM32_NUM_IFACES into the constructor of the virtual CAN driver.
*/
static constexpr unsigned QueuePoolSize =
(UAVCAN_STM32_NUM_IFACES * uavcan::MemPoolBlockSize *MaxCanFramsPerTransfer);
static constexpr unsigned StackSize = 3500;
static constexpr unsigned Priority = 120;
typedef uavcan::SubNode<MemPoolSize> SubNode;
public:
UavcanServers(uavcan::INode &main_node);
virtual ~UavcanServers();
static int start(unsigned num_ifaces, uavcan::INode &main_node);
static int stop(void);
SubNode &get_node() { return _subnode; }
static UavcanServers *instance() { return _instance; }
/*
* Set the main node's pointer to to the injector
* This is a work around as main_node.getDispatcher().remeveRxFrameListener();
* would require a dynamic cast and rtti is not enabled.
*/
void attachITxQueueInjector(ITxQueueInjector **injector) {*injector = &_vdriver;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
private:
pthread_t _subnode_thread;
pthread_mutex_t _subnode_mutex;
int init(unsigned num_ifaces);
void deinit(void);
pthread_addr_t run(pthread_addr_t);
static UavcanServers *_instance; ///< singleton pointer
typedef VirtualCanDriver<QueuePoolSize> vCanDriver;
vCanDriver _vdriver;
uavcan::SubNode<MemPoolSize> _subnode; ///< library instance
uavcan::INode &_main_node; ///< library instance
uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
uavcan_posix::FirmwareVersionChecker _fw_version_checker;
uavcan::dynamic_node_id_server::CentralizedServer _server_instance; ///< server singleton pointer
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
uavcan::NodeInfoRetriever _node_info_retriever;
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
uavcan::BasicFileServer _fw_server;
bool _mutex_inited;
volatile bool _check_fw;
};
src/modules/uavcan/uavcan_virtual_can_driver.hpp
+506
View File
File diff suppressed because it is too large Load Diff