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

Merge pull request #1128 from PX4/uavcan 

Basic UAVCAN support
This commit is contained in:
Lorenz Meier 2014-07-22 06:48:49 +02:00
parent 5bb3e92d36 940becd0c1
commit 086fc7f758
21 changed files with 1424 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitmodules vendored
View File

@ -4,3 +4,6 @@
[submodule "NuttX"]
path = NuttX
url = git://github.com/PX4/NuttX.git
[submodule "uavcan"]
path = uavcan
url = git://github.com/pavel-kirienko/uavcan.git

3
Makefile
View File

@ -212,6 +212,9 @@ endif
$(NUTTX_SRC):
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
$(UAVCAN_DIR):
$(Q) (./Tools/check_submodules.sh)
.PHONY: checksubmodules
checksubmodules:
$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)

27
ROMFS/px4fmu_common/init.d/4012_quad_x_can Normal file
View File

@ -0,0 +1,27 @@
#!nsh
#
# F450-sized quadrotor with CAN
#
# Lorenz Meier <lm@inf.ethz.ch>
#
sh /etc/init.d/4001_quad_x
if [ $DO_AUTOCONFIG == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0.0
fi
set OUTPUT_MODE uavcan_esc

5
ROMFS/px4fmu_common/init.d/rc.autostart
View File

@ -136,6 +136,11 @@ then
sh /etc/init.d/4011_dji_f450
fi
if param compare SYS_AUTOSTART 4012
then
sh /etc/init.d/4012_quad_x_can
fi
if param compare SYS_AUTOSTART 4020
then
sh /etc/init.d/4020_hk_micro_pcb

5
ROMFS/px4fmu_common/init.d/rc.interface
View File

@ -24,6 +24,11 @@ then
else
set OUTPUT_DEV /dev/pwm_output
fi
if [ $OUTPUT_MODE == uavcan_esc ]
then
set OUTPUT_DEV /dev/uavcan/esc
fi
if mixer load $OUTPUT_DEV $MIXER_FILE
then

12
ROMFS/px4fmu_common/init.d/rcS
View File

@ -297,7 +297,17 @@ then
# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
if [ $OUTPUT_MODE != none ]
then
if [ $OUTPUT_MODE == io ]
if [ $OUTPUT_MODE == uavcan_esc ]
then
if uavcan start 1
then
echo "CAN UP"
else
echo "CAN ERR"
fi
fi
if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
then
echo "[init] Use PX4IO PWM as primary output"
if px4io start

24
Tools/check_submodules.sh
View File

@ -53,4 +53,28 @@ else
git submodule update;
fi
if [ -d uavcan ]
then
STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<")
if [ -z "$STATUSRETVAL" ]
then
echo "Checked uavcan submodule, correct version found"
else
echo ""
echo ""
echo "uavcan sub repo not at correct version. Try 'git submodule update'"
echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
echo ""
echo ""
echo "New commits required:"
echo "$(git submodule summary)"
echo ""
exit 1
fi
else
git submodule init
git submodule update
fi
exit 0

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -74,6 +74,7 @@ MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/gpio_led
MODULES += modules/uavcan
#
# Estimation modules (EKF/ SO3 / other filters)

1
makefiles/nuttx.mk
View File

@ -64,6 +64,7 @@ LDSCRIPT += $(NUTTX_EXPORT_DIR)build/ld.script
# Add directories from the NuttX export to the relevant search paths
#
INCLUDE_DIRS += $(NUTTX_EXPORT_DIR)include \
$(NUTTX_EXPORT_DIR)include/cxx \
$(NUTTX_EXPORT_DIR)arch/chip \
$(NUTTX_EXPORT_DIR)arch/common

1
makefiles/setup.mk
View File

@ -49,6 +49,7 @@ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink)/
export UAVCAN_DIR = $(abspath $(PX4_BASE)/uavcan)/
export ROMFS_SRC = $(abspath $(PX4_BASE)/ROMFS)/
export IMAGE_DIR = $(abspath $(PX4_BASE)/Images)/
export BUILD_DIR = $(abspath $(PX4_BASE)/Build)/

2
makefiles/toolchain_gnu-arm-eabi.mk
View File

@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
# Language-specific flags
#
ARCHCFLAGS = -std=gnu99
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x
ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
# Generic warnings
#

1
src/modules/uavcan/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
./dsdlc_generated/

138
src/modules/uavcan/esc_controller.cpp Normal file
View File

@ -0,0 +1,138 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file esc_controller.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "esc_controller.hpp"
#include <systemlib/err.h>
UavcanEscController::UavcanEscController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
_uavcan_sub_status(node),
_orb_timer(node)
{
}
UavcanEscController::~UavcanEscController()
{
perf_free(_perfcnt_invalid_input);
perf_free(_perfcnt_scaling_error);
}
int UavcanEscController::init()
{
// ESC status subscription
int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
if (res < 0)
{
warnx("ESC status sub failed %i", res);
return res;
}
// ESC status will be relayed from UAVCAN bus into ORB at this rate
_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
return res;
}
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
perf_count(_perfcnt_invalid_input);
return;
}
/*
* 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;
/*
* Fill the command message
* If unarmed, we publish an empty message anyway
*/
uavcan::equipment::esc::RawCommand msg;
if (_armed) {
for (unsigned i = 0; i < num_outputs; i++) {
float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
if (scaled < 1.0F) {
scaled = 1.0F; // Since we're armed, we don't want to stop it completely
}
if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
perf_count(_perfcnt_scaling_error);
} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
perf_count(_perfcnt_scaling_error);
} else {
; // Correct value
}
msg.cmd.push_back(static_cast<unsigned>(scaled));
}
}
/*
* Publish the command message to the bus
* Note that for a quadrotor it takes one CAN frame
*/
(void)_uavcan_pub_raw_cmd.broadcast(msg);
}
void UavcanEscController::arm_esc(bool arm)
{
_armed = arm;
}
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
{
// TODO publish to ORB
}

107
src/modules/uavcan/esc_controller.hpp Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file esc_controller.hpp
*
* UAVCAN <--> ORB bridge for ESC messages:
* uavcan.equipment.esc.RawCommand
* uavcan.equipment.esc.RPMCommand
* uavcan.equipment.esc.Status
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <systemlib/perf_counter.h>
class UavcanEscController
{
public:
UavcanEscController(uavcan::INode& node);
~UavcanEscController();
int init();
void update_outputs(float *outputs, unsigned num_outputs);
void arm_esc(bool arm);
private:
/**
* ESC status message reception will be reported via this callback.
*/
void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
/**
* ESC status will be published to ORB from this callback (fixed rate).
*/
void orb_pub_timer_cb(const uavcan::TimerEvent &event);
static constexpr unsigned MAX_RATE_HZ = 100; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
typedef uavcan::MethodBinder<UavcanEscController*,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
StatusCbBinder;
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
/*
* libuavcan related things
*/
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::esc::RawCommand> _uavcan_pub_raw_cmd;
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
bool _armed = false;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters
*/
perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
};

153
src/modules/uavcan/gnss_receiver.cpp Normal file
View File

@ -0,0 +1,153 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file gnss_receiver.cpp
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*
*/
#include "gnss_receiver.hpp"
#include <systemlib/err.h>
#include <mathlib/mathlib.h>
#define MM_PER_CM 10 // Millimeters per centimeter
UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
_node(node),
_uavcan_sub_status(node),
_report_pub(-1)
{
}
int UavcanGnssReceiver::init()
{
int res = -1;
// GNSS fix subscription
res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
if (res < 0)
{
warnx("GNSS fix sub failed %i", res);
return res;
}
// Clear the uORB GPS report
memset(&_report, 0, sizeof(_report));
return res;
}
void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
{
_report.timestamp_position = hrt_absolute_time();
_report.lat = msg.lat_1e7;
_report.lon = msg.lon_1e7;
_report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3)
_report.timestamp_variance = _report.timestamp_position;
// Check if the msg contains valid covariance information
const bool valid_position_covariance = !msg.position_covariance.empty();
const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
if (valid_position_covariance) {
float pos_cov[9];
msg.position_covariance.unpackSquareMatrix(pos_cov);
// Horizontal position uncertainty
const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
// 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);
_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.
* Use Jacobian to transform velocity covariance to heading covariance
*
* Nonlinear equation:
* heading = atan2(vel_e_m_s, vel_n_m_s)
* For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
*
* To calculate the variance of heading from the variance of velocity,
* cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
*/
float vel_n = msg.ned_velocity[0];
float vel_e = msg.ned_velocity[1];
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));
} else {
_report.s_variance_m_s = -1.0F;
_report.c_variance_rad = -1.0F;
}
_report.fix_type = msg.status;
_report.timestamp_velocity = _report.timestamp_position;
_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.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
_report.vel_ned_valid = true;
_report.timestamp_time = _report.timestamp_position;
_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds
_report.satellites_used = msg.sats_used;
if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
} else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
}
}

84
src/modules/uavcan/gnss_receiver.hpp Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file gnss_receiver.hpp
*
* UAVCAN --> ORB bridge for GNSS messages:
* uavcan.equipment.gnss.Fix
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
* @author Andrew Chambers <achamber@gmail.com>
*/
#pragma once
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/gnss/Fix.hpp>
class UavcanGnssReceiver
{
public:
UavcanGnssReceiver(uavcan::INode& node);
int init();
private:
/**
* GNSS fix message will be reported via this callback.
*/
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
typedef uavcan::MethodBinder<UavcanGnssReceiver*,
void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
FixCbBinder;
/*
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status;
/*
* uORB
*/
struct vehicle_gps_position_s _report; ///< uORB topic for gnss position
orb_advert_t _report_pub; ///< uORB pub for gnss position
};

74
src/modules/uavcan/module.mk Normal file
View File

@ -0,0 +1,74 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
# Author: Pavel Kirienko <pavel.kirienko@gmail.com>
#
# 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.
#
############################################################################
#
# UAVCAN <--> uORB bridge
#
MODULE_COMMAND = uavcan
MAXOPTIMIZATION = -Os
SRCS += uavcan_main.cpp \
uavcan_clock.cpp \
esc_controller.cpp \
gnss_receiver.cpp
#
# libuavcan
#
include $(UAVCAN_DIR)/libuavcan/include.mk
SRCS += $(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
#
# libuavcan drivers for STM32
#
include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
SRCS += $(LIBUAVCAN_STM32_SRC)
INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
#
# Invoke DSDL compiler
# TODO: Add make target for this, or invoke dsdlc manually.
# The second option assumes that the generated headers shall be saved
# under the version control, which may be undesirable.
# The first option requires any Python and the Python Mako library for the sources to be built.
#
$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
INCLUDE_DIRS += dsdlc_generated

79
src/modules/uavcan/uavcan_clock.cpp Normal file
View File

@ -0,0 +1,79 @@
/****************************************************************************
*
* 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 <uavcan_stm32/uavcan_stm32.hpp>
#include <drivers/drv_hrt.h>
/**
* @file uavcan_clock.cpp
*
* Implements a clock for the CAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
namespace uavcan_stm32
{
namespace clock
{
uavcan::MonotonicTime getMonotonic()
{
return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
}
uavcan::UtcTime getUtc()
{
return uavcan::UtcTime();
}
void adjustUtc(uavcan::UtcDuration adjustment)
{
(void)adjustment;
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
{
return 0;
}
} // namespace clock
SystemClock &SystemClock::instance()
{
static SystemClock inst;
return inst;
}
}

582
src/modules/uavcan/uavcan_main.cpp Normal file
View File

@ -0,0 +1,582 @@
/****************************************************************************
*
* 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 <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <arch/board/board.h>
#include <arch/chip/chip.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include "uavcan_main.hpp"
/**
* @file uavcan_main.cpp
*
* Implements basic functinality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
/*
* UavcanNode
*/
UavcanNode *UavcanNode::_instance;
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
CDev("uavcan", UAVCAN_DEVICE_PATH),
_node(can_driver, system_clock),
_esc_controller(_node),
_gnss_receiver(_node)
{
_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);
// memset(_controls, 0, sizeof(_controls));
// memset(_poll_fds, 0, sizeof(_poll_fds));
}
UavcanNode::~UavcanNode()
{
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
unsigned i = 10;
do {
/* wait 5ms - it should wake every 10ms or so worst-case */
usleep(5000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
}
/* clean up the alternate device node */
// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
::close(_armed_sub);
_instance = nullptr;
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* GPIO config.
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
* fail during initialization.
*/
stm32_configgpio(GPIO_CAN1_RX);
stm32_configgpio(GPIO_CAN1_TX);
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
stm32_configgpio(GPIO_CAN2_TX);
/*
* CAN driver init
*/
static CanInitHelper can;
static bool can_initialized = false;
if (!can_initialized) {
const int can_init_res = can.init(bitrate);
if (can_init_res < 0) {
warnx("CAN driver init failed %i", can_init_res);
return can_init_res;
}
can_initialized = true;
}
/*
* Node init
*/
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
delete _instance;
_instance = nullptr;
warnx("Node init failed %i", node_init_res);
return node_init_res;
}
/*
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
warnx("start failed: %d", errno);
return -errno;
}
return OK;
}
int UavcanNode::init(uavcan::NodeID node_id)
{
int ret = -1;
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
ret = _esc_controller.init();
if (ret < 0)
return ret;
ret = _gnss_receiver.init();
if (ret < 0)
return ret;
uavcan::protocol::SoftwareVersion swver;
swver.major = 12; // TODO fill version info
swver.minor = 34;
_node.setSoftwareVersion(swver);
uavcan::protocol::HardwareVersion hwver;
hwver.major = 42; // TODO fill version info
hwver.minor = 42;
_node.setHardwareVersion(hwver);
_node.setName("org.pixhawk"); // Huh?
_node.setNodeID(node_id);
return _node.start();
}
void UavcanNode::node_spin_once()
{
const int spin_res = _node.spin(uavcan::MonotonicTime());
if (spin_res < 0) {
warnx("node spin error %i", spin_res);
}
}
int UavcanNode::run()
{
const unsigned PollTimeoutMs = 50;
// XXX figure out the output count
_output_count = 2;
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
actuator_outputs_s outputs;
memset(&outputs, 0, sizeof(outputs));
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
if (busevent_fd < 0)
{
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
_task_should_exit = true;
}
/*
* XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
* IO multiplexing shall be done here.
*/
_node.setStatusOk();
while (!_task_should_exit) {
if (_groups_subscribed != _groups_required) {
subscribe();
_groups_subscribed = _groups_required;
/*
* This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
* Please note that with such multiplexing it is no longer possible to rely only on
* the value returned from poll() to detect whether actuator control has timed out or not.
* Instead, all ORB events need to be checked individually (see below).
*/
_poll_fds[_poll_fds_num] = ::pollfd();
_poll_fds[_poll_fds_num].fd = busevent_fd;
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num += 1;
}
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
node_spin_once(); // Non-blocking
// this would be bad...
if (poll_ret < 0) {
log("poll error %d", errno);
continue;
} else {
// get controls for required topics
bool controls_updated = false;
unsigned poll_id = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
controls_updated = true;
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
poll_id++;
}
}
if (!controls_updated) {
// timeout: no control data, switch to failsafe values
// XXX trigger failsafe
}
//can we mix?
if (controls_updated && (_mixers != nullptr)) {
// XXX one output group has 8 outputs max,
// but this driver could well serve multiple groups.
unsigned num_outputs_max = 8;
// Do mixing
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
outputs.timestamp = hrt_absolute_time();
// iterate actuators
for (unsigned i = 0; i < outputs.noutputs; i++) {
// last resort: catch NaN, INF and out-of-band errors
if (!isfinite(outputs.output[i])) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
// limit outputs to valid range
// never go below min
if (outputs.output[i] < -1.0f) {
outputs.output[i] = -1.0f;
}
// never go below max
if (outputs.output[i] > 1.0f) {
outputs.output[i] = 1.0f;
}
}
// Output to the bus
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
}
}
// Check arming state
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
// Update the armed status and check that we're not locked down
bool set_armed = _armed.armed && !_armed.lockdown;
arm_actuators(set_armed);
}
}
teardown();
warnx("exiting.");
exit(0);
}
int
UavcanNode::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
return 0;
}
int
UavcanNode::teardown()
{
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
::close(_control_subs[i]);
_control_subs[i] = -1;
}
}
return ::close(_armed_sub);
}
int
UavcanNode::arm_actuators(bool arm)
{
_is_armed = arm;
_esc_controller.arm_esc(arm);
return OK;
}
void
UavcanNode::subscribe()
{
// Subscribe/unsubscribe to required actuator control groups
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
_poll_fds_num = 0;
for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (sub_groups & (1 << i)) {
warnx("subscribe to actuator_controls_%d", i);
_control_subs[i] = orb_subscribe(_control_topics[i]);
}
if (unsub_groups & (1 << i)) {
warnx("unsubscribe from actuator_controls_%d", i);
::close(_control_subs[i]);
_control_subs[i] = -1;
}
if (_control_subs[i] > 0) {
_poll_fds[_poll_fds_num].fd = _control_subs[i];
_poll_fds[_poll_fds_num].events = POLLIN;
_poll_fds_num++;
}
}
}
int
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
lock();
switch (cmd) {
case PWM_SERVO_ARM:
arm_actuators(true);
break;
case PWM_SERVO_SET_ARM_OK:
case PWM_SERVO_CLEAR_ARM_OK:
case PWM_SERVO_SET_FORCE_SAFETY_OFF:
// these are no-ops, as no safety switch
break;
case PWM_SERVO_DISARM:
arm_actuators(false);
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _output_count;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
}
break;
case MIXERIOCLOADBUF: {
const char *buf = (const char *)arg;
unsigned buflen = strnlen(buf, 1024);
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
if (_mixers == nullptr) {
_groups_required = 0;
ret = -ENOMEM;
} else {
ret = _mixers->load_from_buf(buf, buflen);
if (ret != 0) {
warnx("mixer load failed with %d", ret);
delete _mixers;
_mixers = nullptr;
_groups_required = 0;
ret = -EINVAL;
} else {
_mixers->groups_required(_groups_required);
}
}
break;
}
default:
ret = -ENOTTY;
break;
}
unlock();
if (ret == -ENOTTY) {
ret = CDev::ioctl(filp, cmd, arg);
}
return ret;
}
void
UavcanNode::print_info()
{
if (!_instance) {
warnx("not running, start first");
}
warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
}
/*
* App entry point
*/
static void print_usage()
{
warnx("usage: uavcan start <node_id> [can_bitrate]");
}
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
int uavcan_main(int argc, char *argv[])
{
constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
if (argc < 2) {
print_usage();
::exit(1);
}
if (!std::strcmp(argv[1], "start")) {
if (argc < 3) {
print_usage();
::exit(1);
}
/*
* Node ID
*/
const int node_id = atoi(argv[2]);
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
warnx("Invalid Node ID %i", node_id);
::exit(1);
}
/*
* CAN bitrate
*/
unsigned bitrate = 0;
if (argc > 3) {
bitrate = atol(argv[3]);
}
if (bitrate <= 0) {
bitrate = DEFAULT_CAN_BITRATE;
}
if (UavcanNode::instance()) {
errx(1, "already started");
}
/*
* Start
*/
warnx("Node ID %u, bitrate %u", node_id, bitrate);
return UavcanNode::start(node_id, bitrate);
}
/* commands below require the app to be started */
UavcanNode *inst = UavcanNode::instance();
if (!inst) {
errx(1, "application not running");
}
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
inst->print_info();
return OK;
}
if (!std::strcmp(argv[1], "stop")) {
delete inst;
inst = nullptr;
return OK;
}
print_usage();
::exit(1);
}

123
src/modules/uavcan/uavcan_main.hpp Normal file
View File

@ -0,0 +1,123 @@
/****************************************************************************
*
* 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 <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include "esc_controller.hpp"
#include "gnss_receiver.hpp"
/**
* @file uavcan_main.hpp
*
* Defines basic functinality of UAVCAN node.
*
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
/**
* A UAVCAN node.
*/
class UavcanNode : public device::CDev
{
static constexpr unsigned MemPoolSize = 10752;
static constexpr unsigned RxQueueLenPerIface = 64;
static constexpr unsigned StackSize = 3000;
public:
typedef uavcan::Node<MemPoolSize> Node;
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
virtual ~UavcanNode();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
static int start(uavcan::NodeID node_id, uint32_t bitrate);
Node& getNode() { return _node; }
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
void subscribe();
int teardown();
int arm_actuators(bool arm);
void print_info();
static UavcanNode* instance() { return _instance; }
private:
int init(uavcan::NodeID node_id);
void node_spin_once();
int run();
int _task = -1; ///< handle to the OS task
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
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
unsigned _output_count = 0; ///< number of actuators currently available
static UavcanNode *_instance; ///< singleton pointer
Node _node; ///< library instance
UavcanEscController _esc_controller;
UavcanGnssReceiver _gnss_receiver;
MixerGroup *_mixers = nullptr;
uint32_t _groups_required = 0;
uint32_t _groups_subscribed = 0;
int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {}; ///< +1 for /dev/uavcan/busevent
unsigned _poll_fds_num = 0;
};

1
uavcan Submodule

@ -0,0 +1 @@
Subproject commit dca2611c3186eaa1cac42557f07b013e2dc633d3