mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-06 15:38:53 +08:00
move uavcan from modules to drivers
This commit is contained in:
Daniel Agar
@@ -1 +0,0 @@
|
||||
./dsdlc_generated/
|
||||
@@ -1,106 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_git_submodule(TARGET git_uavcan PATH "libuavcan")
|
||||
|
||||
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||
|
||||
if(CONFIG_ARCH_CHIP)
|
||||
|
||||
if (${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_PLATFORM "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif (${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_PLATFORM "stm32")
|
||||
set(UAVCAN_TIMER 5) # The default timer the 5
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(NOT config_uavcan_num_ifaces)
|
||||
message(FATAL_ERROR "config_uavcan_num_ifaces not set")
|
||||
endif()
|
||||
|
||||
string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
|
||||
string(TOUPPER "${UAVCAN_PLATFORM}" UAVCAN_PLATFORM_UPPER)
|
||||
add_definitions(
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=${UAVCAN_PLATFORM}
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_PLATFORM}
|
||||
-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_PLATFORM_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
)
|
||||
|
||||
add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
|
||||
add_dependencies(uavcan prebuild_targets)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcan
|
||||
MAIN uavcan
|
||||
STACK_MAIN 3200
|
||||
STACK_MAX 1500
|
||||
INCLUDES
|
||||
${PX4_SOURCE_DIR}/mavlink/include/mavlink
|
||||
libuavcan/libuavcan/include
|
||||
libuavcan/libuavcan/include/dsdlc_generated
|
||||
libuavcan/libuavcan_drivers/posix/include
|
||||
libuavcan/libuavcan_drivers/${UAVCAN_PLATFORM}/driver/include
|
||||
SRCS
|
||||
# Main
|
||||
uavcan_main.cpp
|
||||
uavcan_servers.cpp
|
||||
uavcan_params.c
|
||||
|
||||
# Actuators
|
||||
actuators/esc.cpp
|
||||
actuators/hardpoint.cpp
|
||||
|
||||
# Sensors
|
||||
sensors/sensor_bridge.cpp
|
||||
sensors/gnss.cpp
|
||||
sensors/mag.cpp
|
||||
sensors/baro.cpp
|
||||
|
||||
DEPENDS
|
||||
mixer
|
||||
version
|
||||
|
||||
git_uavcan
|
||||
|
||||
# within libuavcan
|
||||
libuavcan_dsdlc
|
||||
uavcan
|
||||
uavcan_${UAVCAN_PLATFORM}_driver
|
||||
)
|
||||
@@ -1,232 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.cpp
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include "esc.hpp"
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#define MOTOR_BIT(x) (1<<(x))
|
||||
|
||||
UavcanEscController::UavcanEscController(uavcan::INode &node) :
|
||||
_node(node),
|
||||
_uavcan_pub_raw_cmd(node),
|
||||
_uavcan_sub_status(node),
|
||||
_orb_timer(node)
|
||||
{
|
||||
_uavcan_pub_raw_cmd.setPriority(UAVCAN_COMMAND_TRANSFER_PRIORITY);
|
||||
|
||||
if (_perfcnt_invalid_input == nullptr) {
|
||||
errx(1, "uavcan: couldn't allocate _perfcnt_invalid_input");
|
||||
}
|
||||
|
||||
if (_perfcnt_scaling_error == nullptr) {
|
||||
errx(1, "uavcan: couldn't allocate _perfcnt_scaling_error");
|
||||
}
|
||||
}
|
||||
|
||||
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 > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize) ||
|
||||
(num_outputs > esc_status_s::CONNECTED_ESC_MAX)) {
|
||||
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;
|
||||
|
||||
actuator_outputs_s actuator_outputs = {};
|
||||
actuator_outputs.noutputs = num_outputs;
|
||||
actuator_outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
|
||||
const float cmd_min = _run_at_idle_throttle_when_armed ? 1.0F : 0.0F;
|
||||
|
||||
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 minimum
|
||||
if (scaled < cmd_min) {
|
||||
scaled = cmd_min;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
}
|
||||
|
||||
if (scaled > cmd_max) {
|
||||
scaled = cmd_max;
|
||||
perf_count(_perfcnt_scaling_error);
|
||||
}
|
||||
|
||||
msg.cmd.push_back(static_cast<int>(scaled));
|
||||
|
||||
_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
|
||||
actuator_outputs.output[i] = scaled;
|
||||
|
||||
} else {
|
||||
msg.cmd.push_back(static_cast<unsigned>(0));
|
||||
actuator_outputs.output[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Remove channels that are always zero.
|
||||
* The objective of this optimization is to avoid broadcasting multi-frame transfers when a single frame
|
||||
* transfer would be enough. This is a valid optimization as the UAVCAN specification implies that all
|
||||
* non-specified ESC setpoints should be considered zero.
|
||||
* The positive outcome is a (marginally) lower bus traffic and lower CPU load.
|
||||
*
|
||||
* From the standpoint of the PX4 architecture, however, this is a hack. It should be investigated why
|
||||
* the mixer returns more outputs than are actually used.
|
||||
*/
|
||||
for (int index = int(msg.cmd.size()) - 1; index >= _max_number_of_nonzero_outputs; index--) {
|
||||
if (msg.cmd[index] != 0) {
|
||||
_max_number_of_nonzero_outputs = index + 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
msg.cmd.resize(_max_number_of_nonzero_outputs);
|
||||
|
||||
/*
|
||||
* Publish the command message to the bus
|
||||
* Note that for a quadrotor it takes one CAN frame
|
||||
*/
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(msg);
|
||||
|
||||
// Publish actuator outputs
|
||||
if (_actuator_outputs_pub != nullptr) {
|
||||
orb_publish(ORB_ID(actuator_outputs), _actuator_outputs_pub, &actuator_outputs);
|
||||
|
||||
} else {
|
||||
int instance;
|
||||
_actuator_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &actuator_outputs,
|
||||
&instance, ORB_PRIO_DEFAULT);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void UavcanEscController::arm_all_escs(bool arm)
|
||||
{
|
||||
if (arm) {
|
||||
_armed_mask = -1;
|
||||
|
||||
} else {
|
||||
_armed_mask = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::arm_single_esc(int num, bool arm)
|
||||
{
|
||||
if (arm) {
|
||||
_armed_mask = MOTOR_BIT(num);
|
||||
|
||||
} else {
|
||||
_armed_mask = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||
{
|
||||
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
|
||||
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
|
||||
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
|
||||
auto &ref = _esc_status.esc[msg.esc_index];
|
||||
|
||||
ref.esc_address = msg.getSrcNodeID().get();
|
||||
|
||||
ref.esc_voltage = msg.voltage;
|
||||
ref.esc_current = msg.current;
|
||||
ref.esc_temperature = msg.temperature;
|
||||
ref.esc_setpoint = msg.power_rating_pct;
|
||||
ref.esc_rpm = msg.rpm;
|
||||
ref.esc_errorcount = msg.error_count;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -1,119 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.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 <perf/perf_counter.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
public:
|
||||
UavcanEscController(uavcan::INode &node);
|
||||
~UavcanEscController();
|
||||
|
||||
int init();
|
||||
|
||||
void update_outputs(float *outputs, unsigned num_outputs);
|
||||
|
||||
void arm_all_escs(bool arm);
|
||||
void arm_single_esc(int num, bool arm);
|
||||
|
||||
void enable_idle_throttle_when_armed(bool value) { _run_at_idle_throttle_when_armed = value; }
|
||||
|
||||
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 = 200; ///< XXX make this configurable
|
||||
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
|
||||
static constexpr unsigned UAVCAN_COMMAND_TRANSFER_PRIORITY = 5; ///< 0..31, inclusive, 0 - highest, 31 - lowest
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController *,
|
||||
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
|
||||
StatusCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
|
||||
TimerCbBinder;
|
||||
|
||||
bool _armed = false;
|
||||
bool _run_at_idle_throttle_when_armed = false;
|
||||
esc_status_s _esc_status = {};
|
||||
orb_advert_t _esc_status_pub = nullptr;
|
||||
orb_advert_t _actuator_outputs_pub = nullptr;
|
||||
|
||||
/*
|
||||
* 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
|
||||
*/
|
||||
uint32_t _armed_mask = 0;
|
||||
uint8_t _max_number_of_nonzero_outputs = 0;
|
||||
|
||||
/*
|
||||
* 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");
|
||||
};
|
||||
@@ -1,90 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 hardpoint.cpp
|
||||
*
|
||||
* @author Andreas Jochum <Andreas@NicaDrone.com>
|
||||
*/
|
||||
|
||||
#include "hardpoint.hpp"
|
||||
#include <systemlib/err.h>
|
||||
|
||||
UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
|
||||
_node(node),
|
||||
_uavcan_pub_raw_cmd(node),
|
||||
_timer(node)
|
||||
{
|
||||
_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
|
||||
}
|
||||
|
||||
|
||||
UavcanHardpointController::~UavcanHardpointController()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
int UavcanHardpointController::init()
|
||||
{
|
||||
/*
|
||||
* Setup timer and call back function for periodic updates
|
||||
*/
|
||||
_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
|
||||
return 0;
|
||||
}
|
||||
|
||||
void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
|
||||
{
|
||||
_cmd.command = command;
|
||||
_cmd.hardpoint_id = hardpoint_id;
|
||||
|
||||
/*
|
||||
* Publish the command message to the bus
|
||||
*/
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
|
||||
/*
|
||||
* Start the periodic update timer after a command is set
|
||||
*/
|
||||
if (!_timer.isRunning()) {
|
||||
_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
|
||||
}
|
||||
|
||||
}
|
||||
void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
|
||||
{
|
||||
/*
|
||||
* Broadcast command at MAX_RATE_HZ
|
||||
*/
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
}
|
||||
@@ -1,89 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 hardpoint.hpp
|
||||
*
|
||||
* @author Andreas Jochum <Andreas@NicaDrone.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/equipment/hardpoint/Command.hpp>
|
||||
#include <uavcan/equipment/hardpoint/Status.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
/**
|
||||
* @brief The UavcanHardpointController class
|
||||
*/
|
||||
|
||||
class UavcanHardpointController
|
||||
{
|
||||
public:
|
||||
UavcanHardpointController(uavcan::INode &node);
|
||||
~UavcanHardpointController();
|
||||
|
||||
/*
|
||||
* setup periodic updater
|
||||
*/
|
||||
int init();
|
||||
|
||||
|
||||
/*
|
||||
* set command
|
||||
*/
|
||||
void set_command(uint8_t hardpoint_id, uint16_t command);
|
||||
|
||||
private:
|
||||
/*
|
||||
* Max update rate to avoid exessive bus traffic
|
||||
*/
|
||||
static constexpr unsigned MAX_RATE_HZ = 1; ///< XXX make this configurable
|
||||
|
||||
uavcan::equipment::hardpoint::Command _cmd;
|
||||
|
||||
void periodic_update(const uavcan::TimerEvent &);
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanHardpointController *, void (UavcanHardpointController::*)(const uavcan::TimerEvent &)>
|
||||
TimerCbBinder;
|
||||
|
||||
pthread_mutex_t _node_mutex;
|
||||
/*
|
||||
* libuavcan related things
|
||||
*/
|
||||
uavcan::INode &_node;
|
||||
uavcan::Publisher<uavcan::equipment::hardpoint::Command> _uavcan_pub_raw_cmd;
|
||||
uavcan::TimerEventForwarder<TimerCbBinder> _timer;
|
||||
|
||||
};
|
||||
@@ -1,70 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||
|
||||
// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
|
||||
namespace uavcan_node
|
||||
{
|
||||
|
||||
struct AllocatorSynchronizer {
|
||||
const ::irqstate_t state = ::enter_critical_section();
|
||||
~AllocatorSynchronizer() { ::leave_critical_section(state); }
|
||||
};
|
||||
|
||||
struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer> {
|
||||
static constexpr unsigned CapacitySoftLimit = 250;
|
||||
static constexpr unsigned CapacityHardLimit = 500;
|
||||
|
||||
Allocator() :
|
||||
uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
|
||||
{ }
|
||||
|
||||
~Allocator()
|
||||
{
|
||||
if (getNumAllocatedBlocks() > 0) {
|
||||
warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
Submodule src/modules/uavcan/libuavcan deleted from 6174b8c10a
@@ -1,142 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include "baro.hpp"
|
||||
#include <cmath>
|
||||
|
||||
const char *const UavcanBarometerBridge::NAME = "baro";
|
||||
|
||||
UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
|
||||
UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
|
||||
_sub_air_pressure_data(node),
|
||||
_sub_air_temperature_data(node),
|
||||
_reports(2, sizeof(sensor_baro_s))
|
||||
{ }
|
||||
|
||||
int UavcanBarometerBridge::init()
|
||||
{
|
||||
int res = device::CDev::init();
|
||||
|
||||
if (res < 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(sensor_baro_s);
|
||||
sensor_baro_s *baro_buf = reinterpret_cast<sensor_baro_s *>(buffer);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
while (count--) {
|
||||
if (_reports.get(baro_buf)) {
|
||||
ret += sizeof(*baro_buf);
|
||||
baro_buf++;
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
// not supported yet, pretend that everything is ok
|
||||
return OK;
|
||||
}
|
||||
|
||||
default: {
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanBarometerBridge::air_temperature_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
|
||||
{
|
||||
last_temperature_kelvin = msg.static_temperature;
|
||||
}
|
||||
|
||||
void UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
|
||||
{
|
||||
sensor_baro_s report{};
|
||||
|
||||
/*
|
||||
* FIXME HACK
|
||||
* This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
|
||||
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
|
||||
* to use an independent time source (based on hardware TIM5) instead of HRT.
|
||||
* The proper solution is to be developed.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
report.temperature = last_temperature_kelvin - 273.15F;
|
||||
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
|
||||
report.error_count = 0;
|
||||
|
||||
/* TODO get device ID for sensor */
|
||||
report.device_id = 0;
|
||||
|
||||
// add to the ring buffer
|
||||
_reports.force(&report);
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &report);
|
||||
}
|
||||
@@ -1,84 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
|
||||
#include <uavcan/equipment/air_data/StaticPressure.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||
|
||||
class RingBuffer;
|
||||
|
||||
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
public:
|
||||
static const char *const NAME;
|
||||
|
||||
UavcanBarometerBridge(uavcan::INode &node);
|
||||
|
||||
const char *get_name() const override { return NAME; }
|
||||
|
||||
int init() override;
|
||||
|
||||
private:
|
||||
ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
|
||||
|
||||
void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
|
||||
void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
|
||||
void (UavcanBarometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
|
||||
AirPressureCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
|
||||
void (UavcanBarometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
|
||||
AirTemperatureCbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
|
||||
|
||||
ringbuffer::RingBuffer _reports;
|
||||
|
||||
float last_temperature_kelvin = 0.0f;
|
||||
|
||||
};
|
||||
@@ -1,463 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.cpp
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* @author Andrew Chambers <achamber@gmail.com>
|
||||
*
|
||||
*/
|
||||
|
||||
#include "gnss.hpp"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
_node(node),
|
||||
_sub_fix(node),
|
||||
_sub_fix2(node),
|
||||
_pub_fix2(node),
|
||||
_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)),
|
||||
_report_pub(nullptr)
|
||||
{
|
||||
}
|
||||
|
||||
UavcanGnssBridge::~UavcanGnssBridge()
|
||||
{
|
||||
(void) orb_unsubscribe(_orb_sub_gnss);
|
||||
}
|
||||
|
||||
int UavcanGnssBridge::init()
|
||||
{
|
||||
int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix2 pub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix sub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix2 sub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
_orb_to_uavcan_pub_timer.startPeriodic(
|
||||
uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
unsigned UavcanGnssBridge::get_num_redundant_channels() const
|
||||
{
|
||||
return (_receiver_node_id < 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::print_status() const
|
||||
{
|
||||
printf("RX errors: %d, using old Fix: %d, receiver node id: ",
|
||||
_sub_fix.getFailureCount(), int(_old_fix_subscriber_active));
|
||||
|
||||
if (_receiver_node_id < 0) {
|
||||
printf("N/A\n");
|
||||
|
||||
} else {
|
||||
printf("%d\n", _receiver_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
|
||||
{
|
||||
const bool valid_pos_cov = !msg.position_covariance.empty();
|
||||
const bool valid_vel_cov = !msg.velocity_covariance.empty();
|
||||
|
||||
float pos_cov[9];
|
||||
msg.position_covariance.unpackSquareMatrix(pos_cov);
|
||||
|
||||
float vel_cov[9];
|
||||
msg.velocity_covariance.unpackSquareMatrix(vel_cov);
|
||||
|
||||
process_fixx(msg, pos_cov, vel_cov, valid_pos_cov, valid_vel_cov);
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg)
|
||||
{
|
||||
if (_old_fix_subscriber_active) {
|
||||
PX4_WARN("GNSS Fix2 message detected, disabling support for the old Fix message");
|
||||
_sub_fix.stop();
|
||||
_old_fix_subscriber_active = false;
|
||||
_receiver_node_id = -1;
|
||||
}
|
||||
|
||||
float pos_cov[9] {};
|
||||
float vel_cov[9] {};
|
||||
bool valid_covariances = true;
|
||||
|
||||
switch (msg.covariance.size()) {
|
||||
// Scalar matrix
|
||||
case 1: {
|
||||
const auto x = msg.covariance[0];
|
||||
|
||||
pos_cov[0] = x;
|
||||
pos_cov[4] = x;
|
||||
pos_cov[8] = x;
|
||||
|
||||
vel_cov[0] = x;
|
||||
vel_cov[4] = x;
|
||||
vel_cov[8] = x;
|
||||
break;
|
||||
}
|
||||
|
||||
// Diagonal matrix (the most common case)
|
||||
case 6: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[4] = msg.covariance[1];
|
||||
pos_cov[8] = msg.covariance[2];
|
||||
|
||||
vel_cov[0] = msg.covariance[3];
|
||||
vel_cov[4] = msg.covariance[4];
|
||||
vel_cov[8] = msg.covariance[5];
|
||||
break;
|
||||
}
|
||||
|
||||
// Upper triangular matrix.
|
||||
// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
|
||||
// Sub-matrix indexes (empty squares contain velocity-position covariance data):
|
||||
// 0 1 2
|
||||
// 1 6 7
|
||||
// 2 7 11
|
||||
// 15 16 17
|
||||
// 16 18 19
|
||||
// 17 19 20
|
||||
case 21: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[1];
|
||||
pos_cov[4] = msg.covariance[6];
|
||||
pos_cov[5] = msg.covariance[7];
|
||||
pos_cov[6] = msg.covariance[2];
|
||||
pos_cov[7] = msg.covariance[7];
|
||||
pos_cov[8] = msg.covariance[11];
|
||||
|
||||
vel_cov[0] = msg.covariance[15];
|
||||
vel_cov[1] = msg.covariance[16];
|
||||
vel_cov[2] = msg.covariance[17];
|
||||
vel_cov[3] = msg.covariance[16];
|
||||
vel_cov[4] = msg.covariance[18];
|
||||
vel_cov[5] = msg.covariance[19];
|
||||
vel_cov[6] = msg.covariance[17];
|
||||
vel_cov[7] = msg.covariance[19];
|
||||
vel_cov[8] = msg.covariance[20];
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
||||
// Full matrix 6x6.
|
||||
// This code has been carefully optimized by hand. We could use unpackSquareMatrix(), but it's slow.
|
||||
// Sub-matrix indexes (empty squares contain velocity-position covariance data):
|
||||
// 0 1 2
|
||||
// 6 7 8
|
||||
// 12 13 14
|
||||
// 21 22 23
|
||||
// 27 28 29
|
||||
// 33 34 35
|
||||
case 36: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[6];
|
||||
pos_cov[4] = msg.covariance[7];
|
||||
pos_cov[5] = msg.covariance[8];
|
||||
pos_cov[6] = msg.covariance[12];
|
||||
pos_cov[7] = msg.covariance[13];
|
||||
pos_cov[8] = msg.covariance[14];
|
||||
|
||||
vel_cov[0] = msg.covariance[21];
|
||||
vel_cov[1] = msg.covariance[22];
|
||||
vel_cov[2] = msg.covariance[23];
|
||||
vel_cov[3] = msg.covariance[27];
|
||||
vel_cov[4] = msg.covariance[28];
|
||||
vel_cov[5] = msg.covariance[29];
|
||||
vel_cov[6] = msg.covariance[33];
|
||||
vel_cov[7] = msg.covariance[34];
|
||||
vel_cov[8] = msg.covariance[35];
|
||||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
||||
// Either empty or invalid sized, interpret as zero matrix
|
||||
default: {
|
||||
valid_covariances = false;
|
||||
break; // Nothing to do
|
||||
}
|
||||
}
|
||||
|
||||
process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
|
||||
}
|
||||
|
||||
template <typename FixType>
|
||||
void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
|
||||
const float (&pos_cov)[9],
|
||||
const float (&vel_cov)[9],
|
||||
const bool valid_pos_cov,
|
||||
const bool valid_vel_cov)
|
||||
{
|
||||
// This bridge does not support redundant GNSS receivers yet.
|
||||
if (_receiver_node_id < 0) {
|
||||
_receiver_node_id = msg.getSrcNodeID().get();
|
||||
PX4_WARN("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.
|
||||
}
|
||||
}
|
||||
|
||||
auto report = ::vehicle_gps_position_s();
|
||||
|
||||
/*
|
||||
* FIXME HACK
|
||||
* There used to be the following line of code:
|
||||
* report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
|
||||
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
|
||||
* to use an independent time source (based on hardware TIM5) instead of HRT.
|
||||
* The proper solution is to be developed.
|
||||
*/
|
||||
report.timestamp = hrt_absolute_time();
|
||||
|
||||
report.lat = msg.latitude_deg_1e8 / 10;
|
||||
report.lon = msg.longitude_deg_1e8 / 10;
|
||||
report.alt = msg.height_msl_mm;
|
||||
report.alt_ellipsoid = msg.height_ellipsoid_mm;
|
||||
|
||||
if (valid_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_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.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_relative = 0;
|
||||
|
||||
const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
|
||||
|
||||
switch (msg.gnss_time_standard) {
|
||||
case FixType::GNSS_TIME_STANDARD_UTC: {
|
||||
report.time_utc_usec = gnss_ts_usec;
|
||||
break;
|
||||
}
|
||||
|
||||
case FixType::GNSS_TIME_STANDARD_GPS: {
|
||||
if (msg.num_leap_seconds > 0) {
|
||||
report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds + 9;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case FixType::GNSS_TIME_STANDARD_TAI: {
|
||||
if (msg.num_leap_seconds > 0) {
|
||||
report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//TODO px4_clock_settime does nothing on the Snapdragon platform
|
||||
#ifndef __PX4_QURT
|
||||
|
||||
// If we haven't already done so, set the system clock using GPS data
|
||||
if (valid_pos_cov && !_system_clock_set) {
|
||||
timespec ts;
|
||||
memset(&ts, 0, sizeof(ts));
|
||||
// get the whole microseconds
|
||||
ts.tv_sec = report.time_utc_usec / 1000000ULL;
|
||||
// get the remainder microseconds and convert to nanoseconds
|
||||
ts.tv_nsec = (report.time_utc_usec % 1000000ULL) * 1000;
|
||||
px4_clock_settime(CLOCK_REALTIME, &ts);
|
||||
_system_clock_set = true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
report.satellites_used = msg.sats_used;
|
||||
|
||||
// Using PDOP for HDOP and VDOP
|
||||
// Relevant discussion: https://github.com/PX4/Firmware/issues/5153
|
||||
report.hdop = msg.pdop;
|
||||
report.vdop = msg.pdop;
|
||||
|
||||
report.heading = NAN;
|
||||
report.heading_offset = NAN;
|
||||
|
||||
// Publish to a multi-topic
|
||||
int32_t gps_orb_instance;
|
||||
orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_pub, &report, &gps_orb_instance,
|
||||
ORB_PRIO_HIGH);
|
||||
|
||||
// Doing less time critical stuff here
|
||||
if (_orb_to_uavcan_pub_timer.isRunning()) {
|
||||
_orb_to_uavcan_pub_timer.stop();
|
||||
PX4_WARN("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers");
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
|
||||
{
|
||||
if (_orb_sub_gnss < 0) {
|
||||
// ORB subscription stops working if this is relocated into init()
|
||||
_orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
|
||||
if (_orb_sub_gnss < 0) {
|
||||
PX4_WARN("GNSS ORB sub errno %d", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_WARN("GNSS ORB fd %d", _orb_sub_gnss);
|
||||
}
|
||||
|
||||
{
|
||||
bool updated = false;
|
||||
const int res = orb_check(_orb_sub_gnss, &updated);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS ORB check err %d %d", res, errno);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!updated) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
auto orb_msg = ::vehicle_gps_position_s();
|
||||
const int res = orb_copy(ORB_ID(vehicle_gps_position), _orb_sub_gnss, &orb_msg);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS ORB read errno %d", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert to UAVCAN
|
||||
using uavcan::equipment::gnss::Fix2;
|
||||
Fix2 msg;
|
||||
|
||||
msg.gnss_timestamp = uavcan::UtcTime::fromUSec(orb_msg.time_utc_usec);
|
||||
msg.gnss_time_standard = Fix2::GNSS_TIME_STANDARD_UTC;
|
||||
|
||||
msg.longitude_deg_1e8 = std::int64_t(orb_msg.lon) * 10LL;
|
||||
msg.latitude_deg_1e8 = std::int64_t(orb_msg.lat) * 10LL;
|
||||
msg.height_ellipsoid_mm = orb_msg.alt_ellipsoid;
|
||||
msg.height_msl_mm = orb_msg.alt;
|
||||
|
||||
msg.ned_velocity[0] = orb_msg.vel_n_m_s;
|
||||
msg.ned_velocity[1] = orb_msg.vel_e_m_s;
|
||||
msg.ned_velocity[2] = orb_msg.vel_d_m_s;
|
||||
|
||||
msg.sats_used = orb_msg.satellites_used;
|
||||
msg.status = orb_msg.fix_type;
|
||||
// mode skipped
|
||||
// sub mode skipped
|
||||
|
||||
// diagonal covariance matrix
|
||||
msg.covariance.resize(2, orb_msg.eph * orb_msg.eph);
|
||||
msg.covariance.resize(3, orb_msg.epv * orb_msg.epv);
|
||||
msg.covariance.resize(6, orb_msg.s_variance_m_s * orb_msg.s_variance_m_s);
|
||||
|
||||
msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop; // this is a hack :(
|
||||
|
||||
// Publishing now
|
||||
(void) _pub_fix2.broadcast(msg);
|
||||
}
|
||||
@@ -1,116 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.hpp
|
||||
*
|
||||
* UAVCAN <--> ORB bridge for GNSS messages:
|
||||
* uavcan.equipment.gnss.Fix (deprecated, but still supported for backward compatibility)
|
||||
* uavcan.equipment.gnss.Fix2
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* @author Andrew Chambers <achamber@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||
#include <uavcan/equipment/gnss/Fix2.hpp>
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
|
||||
class UavcanGnssBridge : public IUavcanSensorBridge
|
||||
{
|
||||
static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10;
|
||||
|
||||
public:
|
||||
static const char *const NAME;
|
||||
|
||||
UavcanGnssBridge(uavcan::INode &node);
|
||||
~UavcanGnssBridge();
|
||||
|
||||
const char *get_name() const override { return NAME; }
|
||||
|
||||
int init() override;
|
||||
|
||||
unsigned get_num_redundant_channels() const override;
|
||||
|
||||
void print_status() const override;
|
||||
|
||||
private:
|
||||
/**
|
||||
* GNSS fix message will be reported via this callback.
|
||||
*/
|
||||
void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
|
||||
void gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &msg);
|
||||
|
||||
template <typename FixType>
|
||||
void process_fixx(const uavcan::ReceivedDataStructure<FixType> &msg,
|
||||
const float (&pos_cov)[9],
|
||||
const float (&vel_cov)[9],
|
||||
const bool valid_pos_cov,
|
||||
const bool valid_vel_cov);
|
||||
|
||||
void broadcast_from_orb(const uavcan::TimerEvent &);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanGnssBridge *,
|
||||
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
|
||||
FixCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanGnssBridge *,
|
||||
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix2> &) >
|
||||
Fix2CbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanGnssBridge *,
|
||||
void (UavcanGnssBridge::*)(const uavcan::TimerEvent &)>
|
||||
TimerCbBinder;
|
||||
|
||||
uavcan::INode &_node;
|
||||
uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix;
|
||||
uavcan::Subscriber<uavcan::equipment::gnss::Fix2, Fix2CbBinder> _sub_fix2;
|
||||
uavcan::Publisher<uavcan::equipment::gnss::Fix2> _pub_fix2;
|
||||
uavcan::TimerEventForwarder<TimerCbBinder> _orb_to_uavcan_pub_timer;
|
||||
int _receiver_node_id = -1;
|
||||
|
||||
bool _old_fix_subscriber_active = true;
|
||||
|
||||
orb_advert_t _report_pub; ///< uORB pub for gnss position
|
||||
|
||||
int _orb_sub_gnss = -1; ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN
|
||||
|
||||
bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data?
|
||||
};
|
||||
@@ -1,152 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include "mag.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
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)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
|
||||
|
||||
_scale.x_scale = 1.0F;
|
||||
_scale.y_scale = 1.0F;
|
||||
_scale.z_scale = 1.0F;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
static uint64_t last_read = 0;
|
||||
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
|
||||
|
||||
/* buffer must be large enough */
|
||||
unsigned count = buflen / sizeof(struct mag_report);
|
||||
|
||||
if (count < 1) {
|
||||
return -ENOSPC;
|
||||
}
|
||||
|
||||
if (last_read < _report.timestamp) {
|
||||
/* copy report */
|
||||
lock();
|
||||
*mag_buf = _report;
|
||||
last_read = _report.timestamp;
|
||||
unlock();
|
||||
return sizeof(struct mag_report);
|
||||
|
||||
} else {
|
||||
/* no new data available, warn caller */
|
||||
return -EAGAIN;
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case MAGIOCSSCALE: {
|
||||
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;
|
||||
}
|
||||
|
||||
case MAGIOCGEXTERNAL: {
|
||||
return 1; // declare it external rise it's priority and to allow for correct orientation compensation
|
||||
}
|
||||
|
||||
case MAGIOCCALIBRATE:
|
||||
case MAGIOCSRANGE:
|
||||
case MAGIOCEXSTRAP: {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
default: {
|
||||
return CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
|
||||
&msg)
|
||||
{
|
||||
lock();
|
||||
/*
|
||||
* FIXME HACK
|
||||
* This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT.
|
||||
* It stopped working when the time sync feature has been introduced, because it caused libuavcan
|
||||
* to use an independent time source (based on hardware TIM5) instead of HRT.
|
||||
* The proper solution is to be developed.
|
||||
*/
|
||||
_report.timestamp = hrt_absolute_time();
|
||||
_report.device_id = _device_id.devid;
|
||||
_report.is_external = true;
|
||||
|
||||
_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);
|
||||
}
|
||||
@@ -1,70 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
|
||||
|
||||
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
public:
|
||||
static const char *const NAME;
|
||||
|
||||
UavcanMagnetometerBridge(uavcan::INode &node);
|
||||
|
||||
const char *get_name() const override { return NAME; }
|
||||
|
||||
int init() override;
|
||||
|
||||
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::MagneticFieldStrength> &msg);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
|
||||
void (UavcanMagnetometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
|
||||
MagCbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
|
||||
struct mag_calibration_s _scale = {};
|
||||
mag_report _report = {};
|
||||
};
|
||||
@@ -1,166 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
#include <cassert>
|
||||
|
||||
#include "gnss.hpp"
|
||||
#include "mag.hpp"
|
||||
#include "baro.hpp"
|
||||
|
||||
/*
|
||||
* IUavcanSensorBridge
|
||||
*/
|
||||
void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge *> &list)
|
||||
{
|
||||
list.add(new UavcanBarometerBridge(node));
|
||||
list.add(new UavcanMagnetometerBridge(node));
|
||||
list.add(new UavcanGnssBridge(node));
|
||||
}
|
||||
|
||||
/*
|
||||
* UavcanCDevSensorBridgeBase
|
||||
*/
|
||||
UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
|
||||
{
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].node_id >= 0) {
|
||||
(void)unregister_class_devname(_class_devname, _channels[i].class_instance);
|
||||
}
|
||||
}
|
||||
|
||||
delete [] _channels;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
{
|
||||
assert(report != nullptr);
|
||||
|
||||
Channel *channel = nullptr;
|
||||
|
||||
// Checking if such channel already exists
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].node_id == node_id) {
|
||||
channel = _channels + i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// No such channel - try to create one
|
||||
if (channel == nullptr) {
|
||||
if (_out_of_channels) {
|
||||
return; // Give up immediately - saves some CPU time
|
||||
}
|
||||
|
||||
DEVICE_LOG("adding channel %d...", node_id);
|
||||
|
||||
// Search for the first free channel
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
if (_channels[i].node_id < 0) {
|
||||
channel = _channels + i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// No free channels left
|
||||
if (channel == nullptr) {
|
||||
_out_of_channels = true;
|
||||
DEVICE_LOG("out of channels");
|
||||
return;
|
||||
}
|
||||
|
||||
// update device id as we now know our device node_id
|
||||
_device_id.devid_s.address = static_cast<uint8_t>(node_id);
|
||||
|
||||
// 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");
|
||||
(void)unregister_class_devname(_class_devname, class_instance);
|
||||
return;
|
||||
}
|
||||
|
||||
// Publish to the appropriate topic, abort on failure
|
||||
channel->node_id = node_id;
|
||||
channel->class_instance = class_instance;
|
||||
|
||||
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH);
|
||||
|
||||
if (channel->orb_advert == nullptr) {
|
||||
DEVICE_LOG("ADVERTISE FAILED");
|
||||
(void)unregister_class_devname(_class_devname, class_instance);
|
||||
*channel = Channel();
|
||||
return;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::print_status() const
|
||||
{
|
||||
printf("devname: %s\n", _class_devname);
|
||||
|
||||
for (unsigned i = 0; i < _max_channels; i++) {
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,132 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <containers/List.hpp>
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/drv_orb_dev.h>
|
||||
|
||||
/**
|
||||
* A sensor bridge class must implement this interface.
|
||||
*/
|
||||
class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge *>
|
||||
{
|
||||
public:
|
||||
static constexpr unsigned MAX_NAME_LEN = 20;
|
||||
|
||||
virtual ~IUavcanSensorBridge() { }
|
||||
|
||||
/**
|
||||
* Returns ASCII name of the bridge.
|
||||
*/
|
||||
virtual const char *get_name() const = 0;
|
||||
|
||||
/**
|
||||
* Starts the bridge.
|
||||
* @return Non-negative value on success, negative on error.
|
||||
*/
|
||||
virtual int init() = 0;
|
||||
|
||||
/**
|
||||
* Returns number of active redundancy channels.
|
||||
*/
|
||||
virtual unsigned get_num_redundant_channels() const = 0;
|
||||
|
||||
/**
|
||||
* Prints current status in a human readable format to stdout.
|
||||
*/
|
||||
virtual void print_status() const = 0;
|
||||
|
||||
/**
|
||||
* 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);
|
||||
};
|
||||
|
||||
/**
|
||||
* This is the base class for redundant sensors with an independent ORB topic per each redundancy channel.
|
||||
* For example, sensor_mag0, sensor_mag1, etc.
|
||||
*/
|
||||
class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev
|
||||
{
|
||||
struct Channel {
|
||||
int node_id = -1;
|
||||
orb_advert_t orb_advert = nullptr;
|
||||
int class_instance = -1;
|
||||
int orb_instance = -1;
|
||||
};
|
||||
|
||||
const unsigned _max_channels;
|
||||
const char *const _class_devname;
|
||||
const orb_id_t _orb_topic;
|
||||
Channel *const _channels;
|
||||
bool _out_of_channels = false;
|
||||
|
||||
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])
|
||||
{
|
||||
_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
|
||||
_device_id.devid_s.bus = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends one measurement into appropriate ORB topic.
|
||||
* New redundancy channels will be registered automatically.
|
||||
* @param node_id Sensor's Node ID
|
||||
* @param report Pointer to ORB message object
|
||||
*/
|
||||
void publish(const int node_id, const void *report);
|
||||
|
||||
public:
|
||||
virtual ~UavcanCDevSensorBridgeBase();
|
||||
|
||||
unsigned get_num_redundant_channels() const override;
|
||||
|
||||
void print_status() const override;
|
||||
};
|
||||
@@ -1,45 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#if defined(UAVCAN_KINETIS_NUTTX)
|
||||
# include <uavcan_kinetis/uavcan_kinetis.hpp>
|
||||
#elif defined(UAVCAN_STM32_NUTTX)
|
||||
# include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#else
|
||||
# error "Unsupported driver"
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,235 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 uavcan_main.hpp
|
||||
*
|
||||
* Defines basic functinality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* Andreas Jochum <Andreas@NicaDrone.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_config.h>
|
||||
|
||||
#include "uavcan_driver.hpp"
|
||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||
#include <uavcan/protocol/global_time_sync_master.hpp>
|
||||
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
||||
#include <uavcan/protocol/node_status_monitor.hpp>
|
||||
#include <uavcan/protocol/param/GetSet.hpp>
|
||||
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
|
||||
#include <uavcan/protocol/RestartNode.hpp>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
#include <uORB/topics/actuator_direct.h>
|
||||
|
||||
#include "actuators/esc.hpp"
|
||||
#include "actuators/hardpoint.hpp"
|
||||
#include "sensors/sensor_bridge.hpp"
|
||||
|
||||
#include "uavcan_servers.hpp"
|
||||
#include "allocator.hpp"
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
|
||||
|
||||
// we add two to allow for actuator_direct and busevent
|
||||
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
|
||||
|
||||
/**
|
||||
* A UAVCAN node.
|
||||
*/
|
||||
class UavcanNode : public cdev::CDev
|
||||
{
|
||||
static constexpr unsigned MaxBitRatePerSec = 1000000;
|
||||
static constexpr unsigned bitPerFrame = 148;
|
||||
static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
|
||||
static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
|
||||
|
||||
static constexpr unsigned PollTimeoutMs = 3;
|
||||
|
||||
|
||||
/*
|
||||
* This memory is reserved for uavcan to use for queuing CAN frames.
|
||||
* At 1Mbit there is approximately one CAN frame every 145 uS.
|
||||
* The number of buffers sets how long you can go without calling
|
||||
* node_spin_xxxx. Since our task is the only one running and the
|
||||
* driver will light the fd when there is a CAN frame we can nun with
|
||||
* a minimum number of buffers to conserver memory. Each buffer is
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
|
||||
* of ~1 mS
|
||||
* 1000000/200
|
||||
*/
|
||||
|
||||
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
|
||||
static constexpr unsigned StackSize = 2400;
|
||||
|
||||
public:
|
||||
typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
enum eServerAction {None, Start, Stop, CheckFW, Busy};
|
||||
|
||||
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);
|
||||
|
||||
uavcan::Node<> &get_node() { return _node; }
|
||||
|
||||
// TODO: move the actuator mixing stuff into the ESC controller class
|
||||
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();
|
||||
|
||||
void shrink();
|
||||
|
||||
void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
|
||||
|
||||
static UavcanNode *instance() { return _instance; }
|
||||
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
|
||||
int fw_server(eServerAction action);
|
||||
void attachITxQueueInjector(ITxQueueInjector *injector) {_tx_injector = injector;}
|
||||
int list_params(int remote_node_id);
|
||||
int save_params(int remote_node_id);
|
||||
int set_param(int remote_node_id, const char *name, char *value);
|
||||
int get_param(int remote_node_id, const char *name);
|
||||
int reset_node(int remote_node_id);
|
||||
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
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 print_params(uavcan::protocol::param::GetSet::Response &resp);
|
||||
int get_set_param(int nodeid, const char *name, uavcan::protocol::param::GetSet::Request &req);
|
||||
void update_params();
|
||||
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp)
|
||||
{
|
||||
_setget_response = resp;
|
||||
}
|
||||
void free_setget_response(void)
|
||||
{
|
||||
_setget_response = nullptr;
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
|
||||
test_motor_s _test_motor = {};
|
||||
bool _test_in_progress = false;
|
||||
|
||||
unsigned _output_count = 0; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
|
||||
uavcan_node::Allocator _pool_allocator;
|
||||
|
||||
uavcan::Node<> _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
px4_sem_t _server_command_sem;
|
||||
UavcanEscController _esc_controller;
|
||||
UavcanHardpointController _hardpoint_controller;
|
||||
uavcan::GlobalTimeSyncMaster _time_sync_master;
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
uavcan::NodeStatusMonitor _node_status_monitor;
|
||||
|
||||
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];
|
||||
actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
|
||||
orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
|
||||
unsigned _poll_fds_num = 0;
|
||||
int32_t _idle_throttle_when_armed = 0;
|
||||
|
||||
int _actuator_direct_sub = -1; ///< uORB subscription of the actuator_direct topic
|
||||
uint8_t _actuator_direct_poll_fd_num = 0;
|
||||
actuator_direct_s _actuator_direct = {};
|
||||
|
||||
actuator_outputs_s _outputs = {};
|
||||
|
||||
perf_counter_t _perf_control_latency;
|
||||
|
||||
bool _airmode = false;
|
||||
|
||||
// index into _poll_fds for each _control_subs handle
|
||||
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
|
||||
|
||||
void handle_time_sync(const uavcan::TimerEvent &);
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanNode *, void (UavcanNode::*)(const uavcan::TimerEvent &)> TimerCallback;
|
||||
uavcan::TimerEventForwarder<TimerCallback> _master_timer;
|
||||
|
||||
bool _callback_success;
|
||||
uavcan::protocol::param::GetSet::Response *_setget_response;
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
|
||||
void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
|
||||
|
||||
};
|
||||
@@ -1,82 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_config.h>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
|
||||
/**
|
||||
* @file uavcan.hpp
|
||||
*
|
||||
* Public header for the UAVCAN module
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* @author David Sidrane <david_s5@nscdg.com>
|
||||
* @author Holger Steinhaus <holger@steinhaus-home.de>
|
||||
*/
|
||||
|
||||
// firmware paths
|
||||
#define UAVCAN_MAX_PATH_LENGTH (128 + 40)
|
||||
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
|
||||
#define UAVCAN_ROMFS_FW_PATH "/etc/firmware/uavcan"
|
||||
#define UAVCAN_ROMFS_FW_PREFIX "_"
|
||||
|
||||
// logging
|
||||
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
|
||||
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
|
||||
|
||||
// device files
|
||||
// TODO: split IOCTL interface in ESC and node related functionality, then change UAVCAN_DEVICE_PATH to "/dev/uavcan/node"
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
|
||||
#define UAVCAN_ESC_DEVICE_PATH "/dev/uavcan/esc"
|
||||
|
||||
// ioctl interface
|
||||
#define _UAVCAN_IOC(_n) (_IOC(_UAVCAN_IOCBASE, _n))
|
||||
#define _UAVCAN_IOCBASE (0x4000) // IOCTL base for module UAVCAN
|
||||
/*
|
||||
* Query if node identification is in progress. Returns:
|
||||
* EINVAL - not applicable in the current operating mode
|
||||
* ETIME - network discovery complete
|
||||
* OK (0) - network discovery in progress
|
||||
*/
|
||||
#define UAVCAN_IOCG_NODEID_INPROGRESS _UAVCAN_IOC(1)
|
||||
/*
|
||||
* Set hardpoint command. Accepts a pointer to uavcan::equipment::hardpoint::Command; returns nothing.
|
||||
* The pointer may be invalidated once the call returns.
|
||||
*/
|
||||
#define UAVCAN_IOCS_HARDPOINT_SET _UAVCAN_IOC(10)
|
||||
|
||||
// public prototypes
|
||||
extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
|
||||
@@ -1,90 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014-2017 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
/**
|
||||
* UAVCAN mode
|
||||
*
|
||||
* 0 - UAVCAN disabled.
|
||||
* 1 - Enables support for UAVCAN sensors without dynamic node ID allocation and firmware update.
|
||||
* 2 - Enables support for UAVCAN sensors with dynamic node ID allocation and firmware update.
|
||||
* 3 - Enables support for UAVCAN sensors and actuators with dynamic node ID allocation and firmware update. Also sets the motor control outputs to UAVCAN.
|
||||
*
|
||||
* @min 0
|
||||
* @max 3
|
||||
* @value 0 Disabled
|
||||
* @value 1 Sensors Manual Config
|
||||
* @value 2 Sensors Automatic Config
|
||||
* @value 3 Sensors and Actuators (ESCs) Automatic Config
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0);
|
||||
|
||||
/**
|
||||
* UAVCAN Node ID.
|
||||
*
|
||||
* Read the specs at http://uavcan.org to learn more about Node ID.
|
||||
*
|
||||
* @min 1
|
||||
* @max 125
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
|
||||
|
||||
/**
|
||||
* UAVCAN CAN bus bitrate.
|
||||
*
|
||||
* @unit bit/s
|
||||
* @min 20000
|
||||
* @max 1000000
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
|
||||
|
||||
/**
|
||||
* UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
|
||||
*
|
||||
* @boolean
|
||||
* @reboot_required true
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UAVCAN_ESC_IDLT, 1);
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,220 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_config.h>
|
||||
#include "uavcan_driver.hpp"
|
||||
#include <drivers/device/device.h>
|
||||
#include <perf/perf_counter.h>
|
||||
|
||||
#include <uavcan/node/sub_node.hpp>
|
||||
#include <uavcan/protocol/node_status_monitor.hpp>
|
||||
#include <uavcan/protocol/param/GetSet.hpp>
|
||||
#include <uavcan/protocol/param/ExecuteOpcode.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/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/indication/BeepCommand.hpp>
|
||||
#include <uavcan/protocol/enumeration/Begin.hpp>
|
||||
#include <uavcan/protocol/enumeration/Indication.hpp>
|
||||
|
||||
#include "uavcan_module.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>
|
||||
*/
|
||||
|
||||
/**
|
||||
* A UAVCAN Server Sub node.
|
||||
*/
|
||||
class __EXPORT UavcanServers
|
||||
{
|
||||
static constexpr unsigned NumIfaces = 1; // UAVCAN_STM32_NUM_IFACES
|
||||
|
||||
static constexpr unsigned StackSize = 6000;
|
||||
static constexpr unsigned Priority = 120;
|
||||
|
||||
static constexpr unsigned VirtualIfaceBlockAllocationQuota = 80;
|
||||
|
||||
static constexpr float BeepFrequencyGenericIndication = 1000.0F;
|
||||
static constexpr float BeepFrequencySuccess = 2000.0F;
|
||||
static constexpr float BeepFrequencyError = 100.0F;
|
||||
|
||||
public:
|
||||
UavcanServers(uavcan::INode &main_node);
|
||||
|
||||
virtual ~UavcanServers();
|
||||
|
||||
static int start(uavcan::INode &main_node);
|
||||
static int stop();
|
||||
|
||||
uavcan::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; }
|
||||
|
||||
bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
|
||||
|
||||
private:
|
||||
pthread_t _subnode_thread;
|
||||
pthread_mutex_t _subnode_mutex;
|
||||
volatile bool _subnode_thread_should_exit = false;
|
||||
|
||||
int init();
|
||||
|
||||
pthread_addr_t run(pthread_addr_t);
|
||||
|
||||
static UavcanServers *_instance; ///< singleton pointer
|
||||
|
||||
VirtualCanDriver _vdriver;
|
||||
|
||||
uavcan::SubNode<> _subnode;
|
||||
uavcan::INode &_main_node;
|
||||
|
||||
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::BasicFileServerBackend _fileserver_backend;
|
||||
uavcan::NodeInfoRetriever _node_info_retriever;
|
||||
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
|
||||
uavcan::BasicFileServer _fw_server;
|
||||
|
||||
/*
|
||||
* The MAVLink parameter bridge needs to know the maximum parameter index
|
||||
* of each node so that clients can determine when parameter listings have
|
||||
* finished. We do that by querying a node's entire parameter set whenever
|
||||
* a parameter message is received for a node with a zero _param_count,
|
||||
* and storing the count here. If a node doesn't respond, or doesn't have
|
||||
* any parameters, its count will stay at zero and we'll try to query the
|
||||
* set again next time.
|
||||
*
|
||||
* The node's UAVCAN ID is used as the index into the _param_counts array.
|
||||
*/
|
||||
uint8_t _param_counts[128] = {};
|
||||
bool _count_in_progress = false;
|
||||
uint8_t _count_index = 0;
|
||||
|
||||
bool _param_in_progress = false;
|
||||
uint8_t _param_index = 0;
|
||||
bool _param_list_in_progress = false;
|
||||
bool _param_list_all_nodes = false;
|
||||
uint8_t _param_list_node_id = 1;
|
||||
|
||||
uint32_t _param_dirty_bitmap[4] = {};
|
||||
uint8_t _param_save_opcode = 0;
|
||||
|
||||
bool _cmd_in_progress = false;
|
||||
|
||||
// uORB topic handle for MAVLink parameter responses
|
||||
orb_advert_t _param_response_pub = nullptr;
|
||||
orb_advert_t _command_ack_pub = nullptr;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)>
|
||||
ExecuteOpcodeCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
|
||||
void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_count(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
void cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result);
|
||||
|
||||
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _param_getset_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _param_opcode_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::RestartNode, RestartNodeCallback> _param_restartnode_client;
|
||||
void param_count(uavcan::NodeID node_id);
|
||||
void param_opcode(uavcan::NodeID node_id);
|
||||
|
||||
uint8_t get_next_active_node_id(uint8_t base);
|
||||
uint8_t get_next_dirty_node_id(uint8_t base);
|
||||
void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); }
|
||||
void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); }
|
||||
bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); }
|
||||
|
||||
void beep(float frequency);
|
||||
|
||||
bool _mutex_inited = false;
|
||||
volatile bool _check_fw = false;
|
||||
|
||||
// ESC enumeration
|
||||
bool _esc_enumeration_active = false;
|
||||
uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize];
|
||||
uint8_t _esc_enumeration_index = 0;
|
||||
uint8_t _esc_count = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)>
|
||||
EnumerationBeginCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
|
||||
EnumerationIndicationCallback;
|
||||
void cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result);
|
||||
void cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg);
|
||||
void cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
|
||||
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback>
|
||||
_enumeration_indication_sub;
|
||||
uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
|
||||
|
||||
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
|
||||
int copyFw(const char *dst, const char *src);
|
||||
};
|
||||
@@ -1,512 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Pavel Kirienko <pavel.kirienko@zubax.com>
|
||||
* David Sidrane <david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <cstdio>
|
||||
#include <fcntl.h>
|
||||
#include <pthread.h>
|
||||
#include <semaphore.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <uavcan/node/sub_node.hpp>
|
||||
#include <uavcan/protocol/node_status_monitor.hpp>
|
||||
|
||||
/*
|
||||
* General purpose wrapper around os's mutual exclusion
|
||||
* mechanism.
|
||||
*
|
||||
* It supports the
|
||||
*
|
||||
* Note the naming of thier_mutex_ implies that the underlying
|
||||
* mutex is owned by class using the Lock.
|
||||
* and this wrapper provides service to initialize and de initialize
|
||||
* the mutex.
|
||||
*/
|
||||
class Lock
|
||||
{
|
||||
pthread_mutex_t &thier_mutex_;
|
||||
|
||||
public:
|
||||
|
||||
Lock(pthread_mutex_t &m) :
|
||||
thier_mutex_(m)
|
||||
{
|
||||
(void)pthread_mutex_lock(&m);
|
||||
}
|
||||
|
||||
~Lock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&thier_mutex_);
|
||||
}
|
||||
|
||||
static int init(pthread_mutex_t &thier_mutex_)
|
||||
{
|
||||
return pthread_mutex_init(&thier_mutex_, NULL);
|
||||
}
|
||||
|
||||
static int deinit(pthread_mutex_t &thier_mutex_)
|
||||
{
|
||||
return pthread_mutex_destroy(&thier_mutex_);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Generic queue based on the linked list class defined in libuavcan.
|
||||
* This class does not use heap memory.
|
||||
*/
|
||||
template <typename T>
|
||||
class Queue
|
||||
{
|
||||
struct Item : public uavcan::LinkedListNode<Item> {
|
||||
T payload;
|
||||
|
||||
template <typename... Args>
|
||||
Item(Args... args) : payload(args...) { }
|
||||
};
|
||||
|
||||
uavcan::LimitedPoolAllocator allocator_;
|
||||
uavcan::LinkedListRoot<Item> list_;
|
||||
|
||||
public:
|
||||
Queue(uavcan::IPoolAllocator &arg_allocator, std::size_t block_allocation_quota) :
|
||||
allocator_(arg_allocator, block_allocation_quota)
|
||||
{
|
||||
uavcan::IsDynamicallyAllocatable<Item>::check();
|
||||
}
|
||||
|
||||
~Queue()
|
||||
{
|
||||
while (!isEmpty()) {
|
||||
pop();
|
||||
}
|
||||
}
|
||||
|
||||
bool isEmpty() const { return list_.isEmpty(); }
|
||||
|
||||
/**
|
||||
* Creates one item in-place at the end of the list.
|
||||
* Returns true if the item was appended successfully, false if there's not enough memory.
|
||||
* Complexity is O(N) where N is queue length.
|
||||
*/
|
||||
template <typename... Args>
|
||||
bool tryEmplace(Args... args)
|
||||
{
|
||||
// Allocating memory
|
||||
void *const ptr = allocator_.allocate(sizeof(Item));
|
||||
|
||||
if (ptr == nullptr) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Constructing the new item
|
||||
Item *const item = new (ptr) Item(args...);
|
||||
assert(item != nullptr);
|
||||
|
||||
// Inserting the new item at the end of the list
|
||||
Item *p = list_.get();
|
||||
|
||||
if (p == nullptr) {
|
||||
list_.insert(item);
|
||||
|
||||
} else {
|
||||
while (p->getNextListNode() != nullptr) {
|
||||
p = p->getNextListNode();
|
||||
}
|
||||
|
||||
assert(p->getNextListNode() == nullptr);
|
||||
p->setNextListNode(item);
|
||||
assert(p->getNextListNode()->getNextListNode() == nullptr);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Accesses the first element.
|
||||
* Nullptr will be returned if the queue is empty.
|
||||
* Complexity is O(1).
|
||||
*/
|
||||
T *peek() { return isEmpty() ? nullptr : &list_.get()->payload; }
|
||||
const T *peek() const { return isEmpty() ? nullptr : &list_.get()->payload; }
|
||||
|
||||
/**
|
||||
* Removes the first element.
|
||||
* If the queue is empty, nothing will be done and assertion failure will be triggered.
|
||||
* Complexity is O(1).
|
||||
*/
|
||||
void pop()
|
||||
{
|
||||
Item *const item = list_.get();
|
||||
assert(item != nullptr);
|
||||
|
||||
if (item != nullptr) {
|
||||
list_.remove(item);
|
||||
item->~Item();
|
||||
allocator_.deallocate(item);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* Objects of this class are owned by the sub-node thread.
|
||||
* This class does not use heap memory.
|
||||
*/
|
||||
class VirtualCanIface : public uavcan::ICanIface,
|
||||
uavcan::Noncopyable
|
||||
{
|
||||
/**
|
||||
* This class re-defines uavcan::RxCanFrame with flags.
|
||||
* Simple inheritance or composition won't work here, because the 40 byte limit will be exceeded,
|
||||
* rendering this class unusable with Queue<>.
|
||||
*/
|
||||
struct RxItem: public uavcan::CanFrame {
|
||||
const uavcan::MonotonicTime ts_mono;
|
||||
const uavcan::UtcTime ts_utc;
|
||||
const uavcan::CanIOFlags flags;
|
||||
const uint8_t iface_index;
|
||||
|
||||
RxItem(const uavcan::CanRxFrame &arg_frame, uavcan::CanIOFlags arg_flags) :
|
||||
uavcan::CanFrame(arg_frame),
|
||||
ts_mono(arg_frame.ts_mono),
|
||||
ts_utc(arg_frame.ts_utc),
|
||||
flags(arg_flags),
|
||||
iface_index(arg_frame.iface_index)
|
||||
{
|
||||
// Making sure it will fit into a pool block with a pointer prefix
|
||||
static_assert(sizeof(RxItem) <= (uavcan::MemPoolBlockSize - 8), "Bad coder, no coffee");
|
||||
}
|
||||
};
|
||||
|
||||
pthread_mutex_t &common_driver_mutex_;
|
||||
|
||||
uavcan::CanTxQueue prioritized_tx_queue_;
|
||||
Queue<RxItem> rx_queue_;
|
||||
|
||||
int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
|
||||
{
|
||||
Lock lock(common_driver_mutex_);
|
||||
prioritized_tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
|
||||
uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) override
|
||||
{
|
||||
Lock lock(common_driver_mutex_);
|
||||
|
||||
if (rx_queue_.isEmpty()) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const auto item = *rx_queue_.peek();
|
||||
rx_queue_.pop();
|
||||
|
||||
out_frame = item;
|
||||
out_ts_monotonic = item.ts_mono;
|
||||
out_ts_utc = item.ts_utc;
|
||||
out_flags = item.flags;
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int16_t configureFilters(const uavcan::CanFilterConfig *, std::uint16_t) override { return -uavcan::ErrDriver; }
|
||||
uint16_t getNumFilters() const override { return 0; }
|
||||
uint64_t getErrorCount() const override { return 0; }
|
||||
|
||||
public:
|
||||
VirtualCanIface(uavcan::IPoolAllocator &allocator, uavcan::ISystemClock &clock,
|
||||
pthread_mutex_t &arg_mutex, unsigned quota_per_queue) :
|
||||
common_driver_mutex_(arg_mutex),
|
||||
prioritized_tx_queue_(allocator, clock, quota_per_queue),
|
||||
rx_queue_(allocator, quota_per_queue)
|
||||
{
|
||||
}
|
||||
|
||||
~VirtualCanIface()
|
||||
{
|
||||
}
|
||||
|
||||
/**
|
||||
* Note that RX queue overwrites oldest items when overflowed.
|
||||
* Call this from the main thread only.
|
||||
* No additional locking is required.
|
||||
*/
|
||||
void addRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags)
|
||||
{
|
||||
Lock lock(common_driver_mutex_);
|
||||
|
||||
if (!rx_queue_.tryEmplace(frame, flags) && !rx_queue_.isEmpty()) {
|
||||
rx_queue_.pop();
|
||||
(void)rx_queue_.tryEmplace(frame, flags);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this from the main thread only.
|
||||
* No additional locking is required.
|
||||
*/
|
||||
void flushTxQueueTo(uavcan::INode &main_node, std::uint8_t iface_index)
|
||||
{
|
||||
Lock lock(common_driver_mutex_);
|
||||
const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
|
||||
|
||||
while (auto e = prioritized_tx_queue_.peek()) {
|
||||
UAVCAN_TRACE("VirtualCanIface", "TX injection [iface=0x%02x]: %s",
|
||||
unsigned(iface_mask), e->toString().c_str());
|
||||
|
||||
const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
|
||||
uavcan::CanTxQueue::Qos(e->qos), e->flags);
|
||||
|
||||
prioritized_tx_queue_.remove(e);
|
||||
|
||||
if (res <= 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this from the sub-node thread only.
|
||||
* No additional locking is required.
|
||||
*/
|
||||
bool hasDataInRxQueue()
|
||||
{
|
||||
Lock lock(common_driver_mutex_);
|
||||
return !rx_queue_.isEmpty();
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* This interface defines one method that will be called by the main node thread periodically in order to
|
||||
* transfer contents of TX queue of the sub-node into the TX queue of the main node.
|
||||
*/
|
||||
class ITxQueueInjector
|
||||
{
|
||||
public:
|
||||
virtual ~ITxQueueInjector() { }
|
||||
|
||||
/**
|
||||
* Flush contents of TX queues into the main node.
|
||||
* @param main_node Reference to the main node.
|
||||
*/
|
||||
virtual void injectTxFramesInto(uavcan::INode &main_node) = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* Objects of this class are owned by the sub-node thread.
|
||||
* This class does not use heap memory.
|
||||
*/
|
||||
class VirtualCanDriver : public uavcan::ICanDriver,
|
||||
public uavcan::IRxFrameListener,
|
||||
public ITxQueueInjector,
|
||||
uavcan::Noncopyable
|
||||
{
|
||||
class Event
|
||||
{
|
||||
FAR px4_sem_t sem;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
int init()
|
||||
{
|
||||
int rv = px4_sem_init(&sem, 0, 0);
|
||||
|
||||
if (rv == 0) {
|
||||
px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int deinit()
|
||||
{
|
||||
return px4_sem_destroy(&sem);
|
||||
}
|
||||
|
||||
|
||||
Event()
|
||||
{
|
||||
}
|
||||
|
||||
~Event()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
*/
|
||||
|
||||
void waitFor(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
static const int NsPerSec = 1000000000;
|
||||
|
||||
if (duration.isPositive()) {
|
||||
auto abstime = ::timespec();
|
||||
|
||||
if (clock_gettime(CLOCK_REALTIME, &abstime) >= 0) {
|
||||
abstime.tv_nsec += duration.toUSec() * 1000;
|
||||
|
||||
if (abstime.tv_nsec >= NsPerSec) {
|
||||
abstime.tv_sec++;
|
||||
abstime.tv_nsec -= NsPerSec;
|
||||
}
|
||||
|
||||
(void)px4_sem_timedwait(&sem, &abstime);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void signal()
|
||||
{
|
||||
int count;
|
||||
int rv = px4_sem_getvalue(&sem, &count);
|
||||
|
||||
if (rv > 0 && count <= 0) {
|
||||
px4_sem_post(&sem);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
Event event_; ///< Used to unblock the select() call when IO happens.
|
||||
pthread_mutex_t driver_mutex_; ///< Shared across all ifaces
|
||||
uavcan::LazyConstructor<VirtualCanIface> ifaces_[uavcan::MaxCanIfaces];
|
||||
const unsigned num_ifaces_;
|
||||
uavcan::ISystemClock &clock_;
|
||||
|
||||
uavcan::ICanIface *getIface(uint8_t iface_index) override
|
||||
{
|
||||
return (iface_index < num_ifaces_) ? ifaces_[iface_index].operator VirtualCanIface * () : nullptr;
|
||||
}
|
||||
|
||||
uint8_t getNumIfaces() const override { return num_ifaces_; }
|
||||
|
||||
/**
|
||||
* This and other methods of ICanDriver will be invoked by the sub-node thread.
|
||||
*/
|
||||
int16_t select(uavcan::CanSelectMasks &inout_masks,
|
||||
const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces],
|
||||
uavcan::MonotonicTime blocking_deadline) override
|
||||
{
|
||||
bool need_block = (inout_masks.write == 0); // Write queue is infinite
|
||||
|
||||
for (unsigned i = 0; need_block && (i < num_ifaces_); i++) {
|
||||
const bool need_read = inout_masks.read & (1U << i);
|
||||
|
||||
if (need_read && ifaces_[i]->hasDataInRxQueue()) {
|
||||
need_block = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (need_block) {
|
||||
event_.waitFor(blocking_deadline - clock_.getMonotonic());
|
||||
}
|
||||
|
||||
inout_masks = uavcan::CanSelectMasks();
|
||||
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) {
|
||||
const std::uint8_t iface_mask = 1U << i;
|
||||
inout_masks.write |= iface_mask; // Always ready to write
|
||||
|
||||
if (ifaces_[i]->hasDataInRxQueue()) {
|
||||
inout_masks.read |= iface_mask;
|
||||
}
|
||||
}
|
||||
|
||||
return num_ifaces_; // We're always ready to write, hence > 0.
|
||||
}
|
||||
|
||||
/**
|
||||
* This handler will be invoked by the main node thread.
|
||||
*/
|
||||
void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
|
||||
{
|
||||
UAVCAN_TRACE("VirtualCanDriver", "RX [flags=%u]: %s", unsigned(flags), frame.toString().c_str());
|
||||
|
||||
if (frame.iface_index < num_ifaces_) {
|
||||
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
|
||||
event_.signal();
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This method will be invoked by the main node thread.
|
||||
*/
|
||||
void injectTxFramesInto(uavcan::INode &main_node) override
|
||||
{
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) {
|
||||
ifaces_[i]->flushTxQueueTo(main_node, i);
|
||||
}
|
||||
|
||||
event_.signal();
|
||||
}
|
||||
|
||||
public:
|
||||
VirtualCanDriver(unsigned arg_num_ifaces,
|
||||
uavcan::ISystemClock &system_clock,
|
||||
uavcan::IPoolAllocator &allocator,
|
||||
unsigned virtual_iface_block_allocation_quota) :
|
||||
num_ifaces_(arg_num_ifaces),
|
||||
clock_(system_clock)
|
||||
{
|
||||
Lock::init(driver_mutex_);
|
||||
event_.init();
|
||||
|
||||
assert(num_ifaces_ > 0 && num_ifaces_ <= uavcan::MaxCanIfaces);
|
||||
|
||||
const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit
|
||||
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) {
|
||||
ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
|
||||
unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
|
||||
}
|
||||
}
|
||||
|
||||
~VirtualCanDriver()
|
||||
{
|
||||
Lock::deinit(driver_mutex_);
|
||||
event_.deinit();
|
||||
}
|
||||
|
||||
};
|
||||
@@ -1 +0,0 @@
|
||||
./dsdlc_generated/
|
||||
@@ -1,88 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_git_submodule(TARGET git_uavcan PATH "../uavcan/libuavcan")
|
||||
|
||||
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||
set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
|
||||
|
||||
string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
|
||||
add_definitions(
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=stm32
|
||||
-DUAVCAN_STM32_${OS_UPPER}=1
|
||||
-DUAVCAN_STM32_NUM_IFACES=1
|
||||
-DUAVCAN_STM32_TIMER_NUMBER=2
|
||||
-DUAVCAN_USE_CPP03=ON
|
||||
-DUAVCAN_USE_EXTERNAL_SNPRINT
|
||||
)
|
||||
|
||||
set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/modules/uavcan/libuavcan)
|
||||
|
||||
add_subdirectory(${LIBUAVCAN_DIR} uavcanesc_libuavcan)
|
||||
add_dependencies(uavcan prebuild_targets)
|
||||
|
||||
include_directories(${PX4_SOURCE_DIR}/src/modules/systemlib/flashparams)
|
||||
include_directories(${LIBUAVCAN_DIR}/libuavcan/include)
|
||||
include_directories(${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated)
|
||||
include_directories(${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include)
|
||||
include_directories(${LIBUAVCAN_DIR}/libuavcan_drivers/stm32/driver/include)
|
||||
|
||||
include_directories(${PX4_SOURCE_DIR}/src/drivers/bootloaders)
|
||||
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc
|
||||
MAIN uavcanesc
|
||||
STACK_MAIN 4096
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
uavcanesc_main.cpp
|
||||
indication_controller.cpp
|
||||
led.cpp
|
||||
uavcanesc_params.c
|
||||
DEPENDS
|
||||
drivers_bootloaders
|
||||
git_uavcan
|
||||
version
|
||||
|
||||
# within libuavcan
|
||||
libuavcan_dsdlc
|
||||
uavcan
|
||||
uavcan_stm32_driver
|
||||
)
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__commands__cfg
|
||||
MAIN cfg
|
||||
STACK_MAIN 1024
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
esc_cfg.cpp
|
||||
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,58 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <stdint.h>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "esc_cfg.hpp"
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tcfg {yada|yada|yada}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int cfg_main(int argc, char *argv[]);
|
||||
int cfg_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__commands__dc
|
||||
MAIN dc
|
||||
STACK_MAIN 1024
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
esc_dc.cpp
|
||||
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,58 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <stdint.h>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "esc_dc.hpp"
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tdc {yada|yada|yada}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int dc_main(int argc, char *argv[]);
|
||||
int dc_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__commands__rpm
|
||||
MAIN rpm
|
||||
STACK_MAIN 1024
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
esc_rpm.cpp
|
||||
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,58 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <stdint.h>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "esc_rpm.hpp"
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\trpm {yada|yada|yada}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int rpm_main(int argc, char *argv[]);
|
||||
int rpm_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -1,47 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__commands__selftest
|
||||
MAIN selftest
|
||||
STACK_MAIN 1024
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
esc_selftest.cpp
|
||||
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,58 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <stdint.h>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "esc_selftest.hpp"
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tselftest {yada|yada|yada}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int selftest_main(int argc, char *argv[]);
|
||||
int selftest_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -1,46 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__commands__stat
|
||||
MAIN stat
|
||||
STACK_MAIN 1024
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
esc_stat.cpp
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,58 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <stdint.h>
|
||||
#include <cstdlib>
|
||||
|
||||
#include "esc_stat.hpp"
|
||||
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tstat {yada|yada|yada}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int stat_main(int argc, char *argv[]);
|
||||
int stat_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,33 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
@@ -1,85 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "indication_controller.hpp"
|
||||
#include <uavcan/equipment/indication/LightsCommand.hpp>
|
||||
#include "led.hpp"
|
||||
|
||||
namespace
|
||||
{
|
||||
unsigned self_light_index = 0;
|
||||
|
||||
void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand> &msg)
|
||||
{
|
||||
uavcan::uint32_t red = 0;
|
||||
uavcan::uint32_t green = 0;
|
||||
uavcan::uint32_t blue = 0;
|
||||
|
||||
for (auto &cmd : msg.commands) {
|
||||
if (cmd.light_id == self_light_index) {
|
||||
using uavcan::equipment::indication::RGB565;
|
||||
|
||||
red = uavcan::uint32_t(float(cmd.color.red) *
|
||||
(255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F);
|
||||
|
||||
green = uavcan::uint32_t(float(cmd.color.green) *
|
||||
(255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F);
|
||||
|
||||
blue = uavcan::uint32_t(float(cmd.color.blue) *
|
||||
(255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F);
|
||||
|
||||
red = uavcan::min<uavcan::uint32_t>(red, 0xFFU);
|
||||
green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
|
||||
blue = uavcan::min<uavcan::uint32_t>(blue, 0xFFU);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int init_indication_controller(uavcan::INode &node)
|
||||
{
|
||||
static uavcan::Subscriber<uavcan::equipment::indication::LightsCommand> sub_light(node);
|
||||
|
||||
self_light_index = 0;
|
||||
|
||||
int res = 0;
|
||||
|
||||
res = sub_light.start(cb_light_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
|
||||
|
||||
int init_indication_controller(uavcan::INode &node);
|
||||
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include "chip/stm32_tim.h"
|
||||
|
||||
|
||||
#include "led.hpp"
|
||||
|
||||
void rgb_led(int r, int g, int b, int freqs)
|
||||
{
|
||||
|
||||
long fosc = 72000000;
|
||||
long prescale = 2048;
|
||||
long p1s = fosc / prescale;
|
||||
long p0p5s = p1s / 2;
|
||||
stm32_tim_channel_t mode = (stm32_tim_channel_t)(STM32_TIM_CH_OUTPWM | STM32_TIM_CH_POLARITY_NEG);
|
||||
static struct stm32_tim_dev_s *tim = 0;
|
||||
|
||||
if (tim == 0) {
|
||||
tim = stm32_tim_init(3);
|
||||
STM32_TIM_SETMODE(tim, STM32_TIM_MODE_UP);
|
||||
STM32_TIM_SETCLOCK(tim, p1s - 8);
|
||||
STM32_TIM_SETPERIOD(tim, p1s);
|
||||
STM32_TIM_SETCOMPARE(tim, 1, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, 0);
|
||||
STM32_TIM_SETCHANNEL(tim, 1, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 2, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 3, mode);
|
||||
}
|
||||
|
||||
long p = freqs == 0 ? p1s : p1s / freqs;
|
||||
STM32_TIM_SETPERIOD(tim, p);
|
||||
|
||||
p = freqs == 0 ? p1s + 1 : p0p5s / freqs;
|
||||
|
||||
STM32_TIM_SETCOMPARE(tim, 1, (r * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, (g * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, (b * p) / 255);
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
void rgb_led(int r, int g, int b, int freqs);
|
||||
__END_DECLS
|
||||
@@ -1,46 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcanesc__nshterm
|
||||
MAIN nshterm
|
||||
STACK_MAIN 1500
|
||||
COMPILE_FLAGS
|
||||
-Wno-deprecated-declarations
|
||||
SRCS
|
||||
nshterm.c
|
||||
PRIORITY "SCHED_PRIORITY_DEFAULT-30"
|
||||
DEPENDS
|
||||
)
|
||||
|
||||
#
|
||||
@@ -1,128 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Andrew Tridgell
|
||||
*
|
||||
* 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 nshterm.c
|
||||
* start a nsh terminal on a given port. This can be useful for error
|
||||
* handling in startup scripts to start a nsh shell on /dev/ttyACM0
|
||||
* for diagnostics
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <termios.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <errno.h>
|
||||
#include <nshlib/nshlib.h>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
__EXPORT int nshterm_main(int argc, char *argv[]);
|
||||
|
||||
int
|
||||
nshterm_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
printf("Usage: nshterm <device>\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
unsigned retries = 0;
|
||||
int fd = -1;
|
||||
|
||||
/* back off 800 ms to avoid running into the USB setup timing */
|
||||
while (hrt_absolute_time() < 800U * 1000U) {
|
||||
usleep(50000);
|
||||
}
|
||||
|
||||
/* try to bring up the console - stop doing so if the system gets armed */
|
||||
while (true) {
|
||||
|
||||
/* the retries are to cope with the behaviour of /dev/ttyACM0 */
|
||||
/* which may not be ready immediately. */
|
||||
fd = open(argv[1], O_RDWR);
|
||||
|
||||
if (fd != -1) {
|
||||
break;
|
||||
}
|
||||
|
||||
usleep(100000);
|
||||
retries++;
|
||||
}
|
||||
|
||||
if (fd == -1) {
|
||||
perror(argv[1]);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
/* set up the serial port with output processing */
|
||||
|
||||
/* Try to set baud rate */
|
||||
struct termios uart_config;
|
||||
int termios_state;
|
||||
|
||||
/* Back up the original uart configuration to restore it after exit */
|
||||
if ((termios_state = tcgetattr(fd, &uart_config)) < 0) {
|
||||
warnx("ERR get config %s: %d\n", argv[1], termios_state);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* Set ONLCR flag (which appends a CR for every LF) */
|
||||
uart_config.c_oflag |= (ONLCR | OPOST);
|
||||
|
||||
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
|
||||
warnx("ERR set config %s\n", argv[1]);
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* setup standard file descriptors */
|
||||
close(0);
|
||||
close(1);
|
||||
close(2);
|
||||
dup2(fd, 0);
|
||||
dup2(fd, 1);
|
||||
dup2(fd, 2);
|
||||
|
||||
nsh_consolemain(0, NULL);
|
||||
|
||||
close(fd);
|
||||
|
||||
return OK;
|
||||
}
|
||||
@@ -1,45 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2018 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#if defined(UAVCAN_KINETIS_NUTTX)
|
||||
# include <uavcan_kinetis/uavcan_kinetis.hpp>
|
||||
#elif defined(UAVCAN_STM32_NUTTX)
|
||||
# include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#else
|
||||
# error "Unsupported driver"
|
||||
#endif
|
||||
@@ -1,528 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <px4_tasks.h>
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <parameters/param.h>
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <version/version.h>
|
||||
#include <arch/board/board.h>
|
||||
#include <arch/chip/chip.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "uavcanesc_main.hpp"
|
||||
#include "led.hpp"
|
||||
#include "indication_controller.hpp"
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
/**
|
||||
*
|
||||
* Implements basic functionality of UAVCAN esc.
|
||||
*/
|
||||
|
||||
/*
|
||||
* This is the AppImageDescriptor used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc of this application
|
||||
*/
|
||||
|
||||
boot_app_shared_section app_descriptor_t AppDescriptor = {
|
||||
.signature = {APP_DESCRIPTOR_SIGNATURE},
|
||||
.image_crc = 0,
|
||||
.image_size = 0,
|
||||
.vcs_commit = 0,
|
||||
.major_version = APP_VERSION_MAJOR,
|
||||
.minor_version = APP_VERSION_MINOR,
|
||||
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
|
||||
};
|
||||
|
||||
/*
|
||||
* UavcanNode
|
||||
*/
|
||||
UavcanEsc *UavcanEsc::_instance;
|
||||
|
||||
UavcanEsc::UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev(UAVCAN_DEVICE_PATH),
|
||||
active_bitrate(0),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
_fw_update_listner(_node),
|
||||
_reset_timer(_node)
|
||||
{
|
||||
const int res = pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
UavcanEsc::~UavcanEsc()
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
|
||||
}
|
||||
|
||||
int UavcanEsc::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
{
|
||||
|
||||
|
||||
if (_instance != nullptr) {
|
||||
PX4_WARN("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* 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) {
|
||||
PX4_WARN("CAN driver init failed %i", can_init_res);
|
||||
return can_init_res;
|
||||
}
|
||||
|
||||
can_initialized = true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Node init
|
||||
*/
|
||||
_instance = new UavcanEsc(can.driver, uavcan_stm32::SystemClock::instance());
|
||||
|
||||
if (_instance == nullptr) {
|
||||
PX4_WARN("Out of memory");
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
const int node_init_res = _instance->init(node_id);
|
||||
|
||||
if (node_init_res < 0) {
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
PX4_WARN("Node init failed %i", node_init_res);
|
||||
return node_init_res;
|
||||
}
|
||||
|
||||
|
||||
/* Keep the bit rate for reboots on BenginFirmware updates */
|
||||
|
||||
_instance->active_bitrate = bitrate;
|
||||
|
||||
/*
|
||||
* Start the task. Normally it should never exit.
|
||||
*/
|
||||
static auto run_trampoline = [](int, char *[]) {return UavcanEsc::_instance->run();};
|
||||
_instance->_task = px4_task_spawn_cmd("uavcanesc", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
|
||||
static_cast<main_t>(run_trampoline), nullptr);
|
||||
|
||||
if (_instance->_task < 0) {
|
||||
PX4_WARN("start failed: %d", errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void UavcanEsc::fill_node_info()
|
||||
{
|
||||
/* software version */
|
||||
uavcan::protocol::SoftwareVersion swver;
|
||||
|
||||
// Extracting the first 8 hex digits of the git hash and converting them to int
|
||||
char fw_git_short[9] = {};
|
||||
std::memmove(fw_git_short, px4_firmware_version_string(), 8);
|
||||
char *end = nullptr;
|
||||
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
|
||||
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
|
||||
swver.major = AppDescriptor.major_version;
|
||||
swver.minor = AppDescriptor.minor_version;
|
||||
swver.image_crc = AppDescriptor.image_crc;
|
||||
|
||||
PX4_WARN("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
|
||||
|
||||
_node.setSoftwareVersion(swver);
|
||||
|
||||
/* hardware version */
|
||||
uavcan::protocol::HardwareVersion hwver;
|
||||
|
||||
hwver.major = HW_VERSION_MAJOR;
|
||||
hwver.minor = HW_VERSION_MINOR;
|
||||
|
||||
mfguid_t mfgid = {};
|
||||
board_get_mfguid(mfgid);
|
||||
uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
|
||||
|
||||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
static void cb_reboot(const uavcan::TimerEvent &)
|
||||
{
|
||||
px4_systemreset(false);
|
||||
|
||||
}
|
||||
|
||||
void UavcanEsc::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanEsc::BeginFirmwareUpdate::Request>
|
||||
&req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanEsc::BeginFirmwareUpdate::Response> &rsp)
|
||||
{
|
||||
static bool inprogress = false;
|
||||
|
||||
rsp.error = rsp.ERROR_UNKNOWN;
|
||||
|
||||
if (req.image_file_remote_path.path.size()) {
|
||||
rsp.error = rsp.ERROR_IN_PROGRESS;
|
||||
|
||||
if (!inprogress) {
|
||||
inprogress = true;
|
||||
bootloader_app_shared_t shared;
|
||||
shared.bus_speed = active_bitrate;
|
||||
shared.node_id = _node.getNodeID().get();
|
||||
bootloader_app_shared_write(&shared, App);
|
||||
rgb_led(255, 128, 0, 5);
|
||||
_reset_timer.setCallback(cb_reboot);
|
||||
_reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000));
|
||||
rsp.error = rsp.ERROR_OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanEsc::init(uavcan::NodeID node_id)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
// Do regular cdev init
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_node.setName(HW_UAVCAN_NAME);
|
||||
|
||||
_node.setNodeID(node_id);
|
||||
|
||||
fill_node_info();
|
||||
|
||||
const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this,
|
||||
&UavcanEsc::cb_beginfirmware_update));
|
||||
|
||||
if (srv_start_res < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Restart handler
|
||||
*/
|
||||
class RestartRequestHandler: public uavcan::IRestartRequestHandler
|
||||
{
|
||||
bool handleRestartRequest(uavcan::NodeID request_source) override
|
||||
{
|
||||
::syslog(LOG_INFO, "UAVCAN: Restarting by request from %i\n", int(request_source.get()));
|
||||
::usleep(20 * 1000 * 1000);
|
||||
px4_systemreset(false);
|
||||
return true; // Will never be executed BTW
|
||||
}
|
||||
} restart_request_handler;
|
||||
|
||||
void UavcanEsc::node_spin_once()
|
||||
{
|
||||
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||
|
||||
if (spin_res < 0) {
|
||||
PX4_WARN("node spin error %i", spin_res);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
add a fd to the list of polled events. This assumes you want
|
||||
POLLIN for now.
|
||||
*/
|
||||
int UavcanEsc::add_poll_fd(int fd)
|
||||
{
|
||||
int ret = _poll_fds_num;
|
||||
|
||||
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
|
||||
errx(1, "uavcan: too many poll fds, exiting");
|
||||
}
|
||||
|
||||
_poll_fds[_poll_fds_num] = ::pollfd();
|
||||
_poll_fds[_poll_fds_num].fd = fd;
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num += 1;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int UavcanEsc::run()
|
||||
{
|
||||
|
||||
get_node().setRestartRequestHandler(&restart_request_handler);
|
||||
|
||||
while (init_indication_controller(get_node()) < 0) {
|
||||
::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
|
||||
::sleep(1);
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
const unsigned PollTimeoutMs = 50;
|
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||
|
||||
if (busevent_fd < 0) {
|
||||
PX4_WARN("Failed to open %s", uavcan_stm32::BusEvent::DevName);
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
|
||||
* but for now we use adjustUtc with a correction of 0
|
||||
*/
|
||||
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
/*
|
||||
* 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).
|
||||
*/
|
||||
add_poll_fd(busevent_fd);
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// Mutex is unlocked while the thread is blocked on IO multiplexing
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
|
||||
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
|
||||
// this would be bad...
|
||||
if (poll_ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
continue;
|
||||
|
||||
} else {
|
||||
// Do Something
|
||||
}
|
||||
}
|
||||
|
||||
teardown();
|
||||
PX4_WARN("exiting.");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
UavcanEsc::teardown()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
UavcanEsc::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
lock();
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
break;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (ret == -ENOTTY) {
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanEsc::print_info()
|
||||
{
|
||||
if (!_instance) {
|
||||
PX4_WARN("not running, start first");
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
/*
|
||||
* App entry point
|
||||
*/
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tuavcanesc {start|status|stop}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_start(int argc, char *argv[]);
|
||||
|
||||
int uavcannode_start(int argc, char *argv[])
|
||||
{
|
||||
// CAN bitrate
|
||||
int32_t bitrate = 0;
|
||||
// Node ID
|
||||
int32_t node_id = 0;
|
||||
|
||||
// Did the bootloader auto baud and get a node ID Allocated
|
||||
|
||||
bootloader_app_shared_t shared;
|
||||
int valid = bootloader_app_shared_read(&shared, BootLoader);
|
||||
|
||||
if (valid == 0) {
|
||||
|
||||
bitrate = shared.bus_speed;
|
||||
node_id = shared.node_id;
|
||||
|
||||
// Invalidate to prevent deja vu
|
||||
|
||||
bootloader_app_shared_invalidate();
|
||||
|
||||
} else {
|
||||
|
||||
// Node ID
|
||||
(void)param_get(param_find("ESC_NODE_ID"), &node_id);
|
||||
(void)param_get(param_find("ESC_BITRATE"), &bitrate);
|
||||
}
|
||||
|
||||
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
|
||||
PX4_WARN("Invalid Node ID %i", node_id);
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
// Start
|
||||
PX4_WARN("Node ID %u, bitrate %u", node_id, bitrate);
|
||||
int rv = UavcanEsc::start(node_id, bitrate);
|
||||
::sleep(1);
|
||||
return rv;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcanesc_main(int argc, char *argv[]);
|
||||
int uavcanesc_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "start")) {
|
||||
|
||||
if (UavcanEsc::instance()) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
return uavcannode_start(argc, argv);
|
||||
}
|
||||
|
||||
/* commands below require the app to be started */
|
||||
UavcanEsc *const inst = UavcanEsc::instance();
|
||||
|
||||
if (!inst) {
|
||||
PX4_ERR("application not running");
|
||||
::exit(1);
|
||||
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
|
||||
inst->print_info();
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "stop")) {
|
||||
delete inst;
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,149 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#include <drivers/device/device.h>
|
||||
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
|
||||
#include <uavcan/node/timer.hpp>
|
||||
/**
|
||||
*
|
||||
* Defines functionality of UAVCAN ESC node.
|
||||
*/
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
|
||||
|
||||
// we add two to allow for actuator_direct and busevent
|
||||
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
|
||||
|
||||
/**
|
||||
* A UAVCAN node.
|
||||
*/
|
||||
class UavcanEsc : public cdev::CDev
|
||||
{
|
||||
/*
|
||||
* This memory is reserved for uavcan to use as over flow for message
|
||||
* Coming from multiple sources that my not be considered at development
|
||||
* time.
|
||||
*
|
||||
* The call to getNumFreeBlocks will tell how many blocks there are
|
||||
* free -and multiply it times getBlockSize to get the number of bytes
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned MemPoolSize = 2048;
|
||||
|
||||
/*
|
||||
* This memory is reserved for uavcan to use for queuing CAN frames.
|
||||
* At 1Mbit there is approximately one CAN frame every 200 uS.
|
||||
* The number of buffers sets how long you can go without calling
|
||||
* node_spin_xxxx. Since our task is the only one running and the
|
||||
* driver will light the fd when there is a CAN frame we can nun with
|
||||
* a minimum number of buffers to conserver memory. Each buffer is
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a maximum required
|
||||
* poll rate of ~1 mS
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned RxQueueLenPerIface = 5;
|
||||
|
||||
/*
|
||||
* This memory is uses for the tasks stack size
|
||||
*/
|
||||
|
||||
static constexpr unsigned StackSize = 4096;
|
||||
|
||||
public:
|
||||
typedef uavcan::Node<MemPoolSize> Node;
|
||||
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;
|
||||
|
||||
UavcanEsc(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
|
||||
|
||||
virtual ~UavcanEsc();
|
||||
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
static int start(uavcan::NodeID node_id, uint32_t bitrate);
|
||||
|
||||
Node &get_node() { return _node; }
|
||||
|
||||
int teardown();
|
||||
|
||||
void print_info();
|
||||
|
||||
static UavcanEsc *instance() { return _instance; }
|
||||
|
||||
|
||||
/* The bit rate that can be passed back to the bootloader */
|
||||
|
||||
int32_t active_bitrate;
|
||||
|
||||
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
|
||||
|
||||
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
|
||||
static UavcanEsc *_instance; ///< singleton pointer
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
|
||||
unsigned _poll_fds_num = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEsc *,
|
||||
void (UavcanEsc::*)(const uavcan::ReceivedDataStructure<UavcanEsc::BeginFirmwareUpdate::Request> &,
|
||||
uavcan::ServiceResponseDataStructure<UavcanEsc::BeginFirmwareUpdate::Response> &)>
|
||||
BeginFirmwareUpdateCallBack;
|
||||
|
||||
uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateCallBack> _fw_update_listner;
|
||||
void cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanEsc::BeginFirmwareUpdate::Request> &req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanEsc::BeginFirmwareUpdate::Response> &rsp);
|
||||
|
||||
public:
|
||||
|
||||
/* A timer used to reboot after the response is sent */
|
||||
|
||||
uavcan::TimerEventForwarder<void (*)(const uavcan::TimerEvent &)> _reset_timer;
|
||||
|
||||
};
|
||||
@@ -1,59 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
/**
|
||||
* UAVCAN Node ID.
|
||||
*
|
||||
* Read the specs at http://uavcan.org to learn more about Node ID.
|
||||
*
|
||||
* @min 1
|
||||
* @max 125
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ESC_NODE_ID, 120);
|
||||
|
||||
/**
|
||||
* UAVCAN CAN bus bitrate.
|
||||
*
|
||||
* @min 20000
|
||||
* @max 1000000
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(ESC_BITRATE, 1000000);
|
||||
@@ -1 +0,0 @@
|
||||
./dsdlc_generated/
|
||||
@@ -1,89 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 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
|
||||
# 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_git_submodule(TARGET git_uavcan PATH "../uavcan/libuavcan")
|
||||
|
||||
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||
set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
|
||||
|
||||
string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
|
||||
add_definitions(
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=stm32
|
||||
-DUAVCAN_STM32_${OS_UPPER}=1
|
||||
-DUAVCAN_STM32_NUM_IFACES=1
|
||||
-DUAVCAN_STM32_TIMER_NUMBER=2
|
||||
-DUAVCAN_USE_CPP03=ON
|
||||
-DUAVCAN_USE_EXTERNAL_SNPRINT
|
||||
)
|
||||
|
||||
set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/modules/uavcan/libuavcan)
|
||||
|
||||
add_subdirectory(${LIBUAVCAN_DIR} uavcannode_libuavcan)
|
||||
add_dependencies(uavcan prebuild_targets)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcannode
|
||||
MAIN uavcannode
|
||||
STACK_MAIN 1048
|
||||
INCLUDES
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include
|
||||
${LIBUAVCAN_DIR}/libuavcan_drivers/stm32/driver/include
|
||||
COMPILE_FLAGS
|
||||
-Wframe-larger-than=1500
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
SRCS
|
||||
uavcannode_main.cpp
|
||||
indication_controller.cpp
|
||||
sim_controller.cpp
|
||||
led.cpp
|
||||
resources.cpp
|
||||
uavcannode_params.c
|
||||
DEPENDS
|
||||
drivers_bootloaders
|
||||
git_uavcan
|
||||
version
|
||||
|
||||
# within libuavcan
|
||||
libuavcan_dsdlc
|
||||
uavcan
|
||||
uavcan_stm32_driver
|
||||
)
|
||||
|
||||
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES uavcan_stm32_driver)
|
||||
@@ -1,96 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "indication_controller.hpp"
|
||||
#include <uavcan/equipment/indication/LightsCommand.hpp>
|
||||
#include "led.hpp"
|
||||
|
||||
namespace
|
||||
{
|
||||
unsigned self_light_index = 0;
|
||||
|
||||
void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand> &msg)
|
||||
{
|
||||
uavcan::uint32_t red = 0;
|
||||
uavcan::uint32_t green = 0;
|
||||
uavcan::uint32_t blue = 0;
|
||||
|
||||
for (auto &cmd : msg.commands) {
|
||||
if (cmd.light_id == self_light_index) {
|
||||
using uavcan::equipment::indication::RGB565;
|
||||
|
||||
red = uavcan::uint32_t(float(cmd.color.red) *
|
||||
(255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F);
|
||||
|
||||
green = uavcan::uint32_t(float(cmd.color.green) *
|
||||
(255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F);
|
||||
|
||||
blue = uavcan::uint32_t(float(cmd.color.blue) *
|
||||
(255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F);
|
||||
|
||||
red = uavcan::min<uavcan::uint32_t>(red, 0xFFU);
|
||||
green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
|
||||
blue = uavcan::min<uavcan::uint32_t>(blue, 0xFFU);
|
||||
}
|
||||
|
||||
if (cmd.light_id == self_light_index + 1) {
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "rgb:%d %d %d hz %d\n", red, green, blue, int(cmd.color.red));
|
||||
}
|
||||
|
||||
rgb_led(red, green, blue, int(cmd.color.red));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int init_indication_controller(uavcan::INode &node)
|
||||
{
|
||||
static uavcan::Subscriber<uavcan::equipment::indication::LightsCommand> sub_light(node);
|
||||
|
||||
self_light_index = 0;
|
||||
|
||||
int res = 0;
|
||||
|
||||
res = sub_light.start(cb_light_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
|
||||
|
||||
int init_indication_controller(uavcan::INode &node);
|
||||
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
#include "chip/stm32_tim.h"
|
||||
|
||||
|
||||
#include "led.hpp"
|
||||
|
||||
void rgb_led(int r, int g, int b, int freqs)
|
||||
{
|
||||
|
||||
long fosc = 72000000;
|
||||
long prescale = 2048;
|
||||
long p1s = fosc / prescale;
|
||||
long p0p5s = p1s / 2;
|
||||
stm32_tim_channel_t mode = (stm32_tim_channel_t)(STM32_TIM_CH_OUTPWM | STM32_TIM_CH_POLARITY_NEG);
|
||||
static struct stm32_tim_dev_s *tim = 0;
|
||||
|
||||
if (tim == 0) {
|
||||
tim = stm32_tim_init(3);
|
||||
STM32_TIM_SETMODE(tim, STM32_TIM_MODE_UP);
|
||||
STM32_TIM_SETCLOCK(tim, p1s - 8);
|
||||
STM32_TIM_SETPERIOD(tim, p1s);
|
||||
STM32_TIM_SETCOMPARE(tim, 1, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, 0);
|
||||
STM32_TIM_SETCHANNEL(tim, 1, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 2, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 3, mode);
|
||||
}
|
||||
|
||||
long p = freqs == 0 ? p1s : p1s / freqs;
|
||||
STM32_TIM_SETPERIOD(tim, p);
|
||||
|
||||
p = freqs == 0 ? p1s + 1 : p0p5s / freqs;
|
||||
|
||||
STM32_TIM_SETCOMPARE(tim, 1, (r * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, (g * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, (b * p) / 255);
|
||||
}
|
||||
@@ -1,37 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
void rgb_led(int r, int g, int b, int freqs);
|
||||
__END_DECLS
|
||||
@@ -1,189 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <nuttx/progmem.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdlib.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
#include "resources.hpp"
|
||||
|
||||
#if !defined(CONFIG_NSH_LIBRARY)
|
||||
|
||||
static const char *
|
||||
tstate_name(const tstate_t s)
|
||||
{
|
||||
switch (s) {
|
||||
case TSTATE_TASK_INVALID: return "init";
|
||||
|
||||
case TSTATE_TASK_PENDING: return "PEND";
|
||||
|
||||
case TSTATE_TASK_READYTORUN: return "READY";
|
||||
|
||||
case TSTATE_TASK_RUNNING: return "RUN";
|
||||
|
||||
case TSTATE_TASK_INACTIVE: return "inact";
|
||||
|
||||
case TSTATE_WAIT_SEM: return "w:sem";
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
|
||||
case TSTATE_WAIT_SIG: return "w:sig";
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
|
||||
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
|
||||
|
||||
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
|
||||
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
|
||||
#endif
|
||||
|
||||
default:
|
||||
return "ERROR";
|
||||
}
|
||||
}
|
||||
|
||||
void stack_check(void)
|
||||
{
|
||||
|
||||
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
|
||||
if (system_load.tasks[i].tcb) {
|
||||
size_t stack_size = system_load.tasks[i].tcb->adj_stack_size;
|
||||
ssize_t stack_free = 0;
|
||||
|
||||
#if CONFIG_ARCH_INTERRUPTSTACK > 3
|
||||
|
||||
if (system_load.tasks[i].tcb->pid == 0) {
|
||||
stack_size = (CONFIG_ARCH_INTERRUPTSTACK & ~3);
|
||||
stack_free = up_check_intstack_remain();
|
||||
|
||||
} else {
|
||||
#endif
|
||||
stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb);
|
||||
#if CONFIG_ARCH_INTERRUPTSTACK > 3
|
||||
}
|
||||
|
||||
#endif
|
||||
::syslog(LOG_INFO, "%4d %*-s %8lld %5u/%5u %3u (%3u) ",
|
||||
system_load.tasks[i].tcb->pid,
|
||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
stack_size - stack_free,
|
||||
stack_size,
|
||||
system_load.tasks[i].tcb->sched_priority,
|
||||
system_load.tasks[i].tcb->base_priority);
|
||||
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
/* print scheduling info with RR time slice */
|
||||
::syslog(LOG_INFO, " %6d\n", system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
/* print task state instead */
|
||||
::syslog(LOG_INFO, " %-6s\n", tstate_name((tstate_t)system_load.tasks[i].tcb->task_state));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void free_getprogmeminfo(struct mallinfo *mem)
|
||||
{
|
||||
size_t page = 0, stpage = 0xFFFF;
|
||||
size_t pagesize = 0;
|
||||
ssize_t status;
|
||||
|
||||
mem->arena = 0;
|
||||
mem->fordblks = 0;
|
||||
mem->uordblks = 0;
|
||||
mem->mxordblk = 0;
|
||||
|
||||
for (status = 0, page = 0; status >= 0; page++) {
|
||||
status = up_progmem_ispageerased(page);
|
||||
pagesize = up_progmem_pagesize(page);
|
||||
|
||||
mem->arena += pagesize;
|
||||
|
||||
/* Is this beginning of new free space section */
|
||||
|
||||
if (status == 0) {
|
||||
if (stpage == 0xFFFF) { stpage = page; }
|
||||
|
||||
mem->fordblks += pagesize;
|
||||
|
||||
} else if (status != 0) {
|
||||
mem->uordblks += pagesize;
|
||||
|
||||
if (stpage != 0xFFFF && up_progmem_isuniform()) {
|
||||
stpage = page - stpage;
|
||||
|
||||
if (stpage > (size_t) mem->mxordblk) {
|
||||
mem->mxordblk = stpage;
|
||||
}
|
||||
|
||||
stpage = 0xFFFF;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mem->mxordblk *= pagesize;
|
||||
}
|
||||
|
||||
void free_check(void)
|
||||
{
|
||||
struct mallinfo data;
|
||||
struct mallinfo prog;
|
||||
|
||||
#ifdef CONFIG_CAN_PASS_STRUCTS
|
||||
data = mallinfo();
|
||||
#else
|
||||
(void)mallinfo(&data);
|
||||
#endif
|
||||
|
||||
free_getprogmeminfo(&prog);
|
||||
|
||||
|
||||
::syslog(LOG_INFO, " total used free largest\n");
|
||||
|
||||
::syslog(LOG_INFO, "Data: %11d%11d%11d%11d\n",
|
||||
data.arena, data.uordblks, data.fordblks, data.mxordblk);
|
||||
|
||||
::syslog(LOG_INFO, "Prog: %11d%11d%11d%11d\n",
|
||||
prog.arena, prog.uordblks, prog.fordblks, prog.mxordblk);
|
||||
}
|
||||
#endif /* CONFIG_NSH_LIBRARY */
|
||||
@@ -1,43 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
#if defined(CONFIG_NSH_LIBRARY)
|
||||
#define stack_check()
|
||||
#define free_check() free_main(0,0)
|
||||
#else
|
||||
void stack_check(void);
|
||||
void free_check(void);
|
||||
#endif
|
||||
__END_DECLS
|
||||
@@ -1,156 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_config.h>
|
||||
|
||||
#include <syslog.h>
|
||||
|
||||
#include "sim_controller.hpp"
|
||||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/esc/RPMCommand.hpp>
|
||||
#include <uavcan/equipment/esc/Status.hpp>
|
||||
#include "led.hpp"
|
||||
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::esc::Status> *pub_status;
|
||||
namespace
|
||||
{
|
||||
unsigned self_index = 0;
|
||||
int rpm = 0;
|
||||
|
||||
static void cb_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand> &msg)
|
||||
{
|
||||
if (msg.cmd.size() <= self_index) {
|
||||
rgb_led(0, 0, 0, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
const float scaled = msg.cmd[self_index] / float(
|
||||
uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max());
|
||||
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "scaled:%d\n", (int)scaled);
|
||||
}
|
||||
|
||||
if (scaled > 0) {
|
||||
} else {
|
||||
}
|
||||
}
|
||||
|
||||
static void cb_rpm_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand> &msg)
|
||||
{
|
||||
if (msg.rpm.size() <= self_index) {
|
||||
return;
|
||||
}
|
||||
|
||||
rpm = msg.rpm[self_index];
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "rpm:%d\n", rpm);
|
||||
}
|
||||
|
||||
if (rpm > 0) {
|
||||
rgb_led(255, 0, 0, rpm);
|
||||
|
||||
} else {
|
||||
rgb_led(0, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void cb_10Hz(const uavcan::TimerEvent &event)
|
||||
{
|
||||
uavcan::equipment::esc::Status msg;
|
||||
|
||||
msg.esc_index = self_index;
|
||||
msg.rpm = rpm;
|
||||
msg.voltage = 3.3F;
|
||||
msg.current = 0.001F;
|
||||
msg.temperature = 24.0F;
|
||||
msg.power_rating_pct = static_cast<unsigned>(.5F * 100 + 0.5F);
|
||||
msg.error_count = 0;
|
||||
|
||||
if (rpm != 0) {
|
||||
// Lower the publish rate to 1Hz if the motor is not running
|
||||
static uavcan::MonotonicTime prev_pub_ts;
|
||||
|
||||
if ((event.scheduled_time - prev_pub_ts).toMSec() >= 990) {
|
||||
prev_pub_ts = event.scheduled_time;
|
||||
pub_status->broadcast(msg);
|
||||
}
|
||||
|
||||
} else {
|
||||
pub_status->broadcast(msg);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
int init_sim_controller(uavcan::INode &node)
|
||||
{
|
||||
|
||||
typedef void (*cb)(const uavcan::TimerEvent &);
|
||||
static uavcan::Subscriber<uavcan::equipment::esc::RawCommand> sub_raw_command(node);
|
||||
static uavcan::Subscriber<uavcan::equipment::esc::RPMCommand> sub_rpm_command(node);
|
||||
static uavcan::TimerEventForwarder<cb> timer_10hz(node);
|
||||
|
||||
self_index = 0;
|
||||
|
||||
int res = 0;
|
||||
|
||||
res = sub_raw_command.start(cb_raw_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
res = sub_rpm_command.start(cb_rpm_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
pub_status = new uavcan::Publisher<uavcan::equipment::esc::Status>(node);
|
||||
res = pub_status->init();
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
timer_10hz.setCallback(&cb_10Hz);
|
||||
timer_10hz.startPeriodic(uavcan::MonotonicDuration::fromMSec(100));
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -1,40 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane<david_s5@nscdg.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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
|
||||
int init_sim_controller(uavcan::INode &node);
|
||||
@@ -1,604 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
*
|
||||
* 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 <px4_config.h>
|
||||
#include <px4_log.h>
|
||||
#include <px4_tasks.h>
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <nuttx/clock.h>
|
||||
#
|
||||
#else
|
||||
#include <px4_workqueue.h>
|
||||
#endif
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <parameters/param.h>
|
||||
#include <lib/mixer/mixer.h>
|
||||
#include <version/version.h>
|
||||
__BEGIN_DECLS
|
||||
#include <nuttx/board.h>
|
||||
#include <arch/chip/chip.h>
|
||||
__END_DECLS
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "uavcannode_main.hpp"
|
||||
#include "indication_controller.hpp"
|
||||
#include "sim_controller.hpp"
|
||||
#include "resources.hpp"
|
||||
#include "led.hpp"
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
*
|
||||
* Implements basic functionality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#define RESOURCE_DEBUG
|
||||
#if defined(RESOURCE_DEBUG)
|
||||
#define resources(s) ::syslog(LOG_INFO," %s\n",(s)); \
|
||||
if (UavcanNode::instance()) { \
|
||||
syslog(LOG_INFO,"UAVCAN getPeakNumUsedBlocks() in bytes %d\n", \
|
||||
UAVCAN_MEM_POOL_BLOCK_SIZE * UavcanNode::instance()->get_node().getAllocator().getPeakNumUsedBlocks()); \
|
||||
} \
|
||||
free_check(); \
|
||||
stack_check();
|
||||
#else
|
||||
#define resources(s)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* This is the AppImageDescriptor used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc of this application
|
||||
*/
|
||||
|
||||
boot_app_shared_section app_descriptor_t AppDescriptor = {
|
||||
.signature = {APP_DESCRIPTOR_SIGNATURE},
|
||||
.image_crc = 0,
|
||||
.image_size = 0,
|
||||
.vcs_commit = 0,
|
||||
.major_version = APP_VERSION_MAJOR,
|
||||
.minor_version = APP_VERSION_MINOR,
|
||||
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
|
||||
};
|
||||
|
||||
/*
|
||||
* UavcanNode
|
||||
*/
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev(UAVCAN_DEVICE_PATH),
|
||||
active_bitrate(0),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
_time_sync_slave(_node),
|
||||
_fw_update_listner(_node),
|
||||
_reset_timer(_node)
|
||||
{
|
||||
const int res = pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
|
||||
}
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
{
|
||||
|
||||
|
||||
if (_instance != nullptr) {
|
||||
warnx("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* 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;
|
||||
}
|
||||
|
||||
|
||||
resources("Before _instance->init:");
|
||||
const int node_init_res = _instance->init(node_id);
|
||||
resources("After _instance->init:");
|
||||
|
||||
if (node_init_res < 0) {
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
warnx("Node init failed %i", node_init_res);
|
||||
return node_init_res;
|
||||
}
|
||||
|
||||
|
||||
/* Keep the bit rate for reboots on BenginFirmware updates */
|
||||
|
||||
_instance->active_bitrate = bitrate;
|
||||
|
||||
/*
|
||||
* Start the task. Normally it should never exit.
|
||||
*/
|
||||
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
|
||||
_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
|
||||
static_cast<main_t>(run_trampoline), nullptr);
|
||||
|
||||
if (_instance->_task < 0) {
|
||||
warnx("start failed: %d", errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void UavcanNode::fill_node_info()
|
||||
{
|
||||
/* software version */
|
||||
uavcan::protocol::SoftwareVersion swver;
|
||||
|
||||
// Extracting the first 8 hex digits of the git hash and converting them to int
|
||||
char fw_git_short[9] = {};
|
||||
std::memmove(fw_git_short, px4_firmware_version_string(), 8);
|
||||
char *end = nullptr;
|
||||
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
|
||||
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
|
||||
swver.major = AppDescriptor.major_version;
|
||||
swver.minor = AppDescriptor.minor_version;
|
||||
swver.image_crc = AppDescriptor.image_crc;
|
||||
|
||||
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
|
||||
|
||||
_node.setSoftwareVersion(swver);
|
||||
|
||||
/* hardware version */
|
||||
uavcan::protocol::HardwareVersion hwver;
|
||||
|
||||
hwver.major = HW_VERSION_MAJOR;
|
||||
hwver.minor = HW_VERSION_MINOR;
|
||||
|
||||
mfguid_t mfgid = {};
|
||||
board_get_mfguid(mfgid);
|
||||
uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
|
||||
|
||||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
static void cb_reboot(const uavcan::TimerEvent &)
|
||||
{
|
||||
px4_systemreset(false);
|
||||
|
||||
}
|
||||
|
||||
void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request>
|
||||
&req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp)
|
||||
{
|
||||
static bool inprogress = false;
|
||||
|
||||
rsp.error = rsp.ERROR_UNKNOWN;
|
||||
|
||||
if (req.image_file_remote_path.path.size()) {
|
||||
rsp.error = rsp.ERROR_IN_PROGRESS;
|
||||
|
||||
if (!inprogress) {
|
||||
inprogress = true;
|
||||
bootloader_app_shared_t shared;
|
||||
shared.bus_speed = active_bitrate;
|
||||
shared.node_id = _node.getNodeID().get();
|
||||
bootloader_app_shared_write(&shared, App);
|
||||
rgb_led(255, 128, 0, 5);
|
||||
_reset_timer.setCallback(cb_reboot);
|
||||
_reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000));
|
||||
rsp.error = rsp.ERROR_OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
// Do regular cdev init
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_node.setName(HW_UAVCAN_NAME);
|
||||
|
||||
_node.setNodeID(node_id);
|
||||
|
||||
fill_node_info();
|
||||
|
||||
const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this,
|
||||
&UavcanNode::cb_beginfirmware_update));
|
||||
|
||||
if (srv_start_res < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Restart handler
|
||||
*/
|
||||
class RestartRequestHandler: public uavcan::IRestartRequestHandler
|
||||
{
|
||||
bool handleRestartRequest(uavcan::NodeID request_source) override
|
||||
{
|
||||
::syslog(LOG_INFO, "UAVCAN: Restarting by request from %i\n", int(request_source.get()));
|
||||
::usleep(20 * 1000 * 1000);
|
||||
px4_systemreset(false);
|
||||
return true; // Will never be executed BTW
|
||||
}
|
||||
} restart_request_handler;
|
||||
|
||||
void UavcanNode::node_spin_once()
|
||||
{
|
||||
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||
|
||||
if (spin_res < 0) {
|
||||
warnx("node spin error %i", spin_res);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
add a fd to the list of polled events. This assumes you want
|
||||
POLLIN for now.
|
||||
*/
|
||||
int UavcanNode::add_poll_fd(int fd)
|
||||
{
|
||||
int ret = _poll_fds_num;
|
||||
|
||||
if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
|
||||
errx(1, "uavcan: too many poll fds, exiting");
|
||||
}
|
||||
|
||||
_poll_fds[_poll_fds_num] = ::pollfd();
|
||||
_poll_fds[_poll_fds_num].fd = fd;
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num += 1;
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
int UavcanNode::run()
|
||||
{
|
||||
|
||||
get_node().setRestartRequestHandler(&restart_request_handler);
|
||||
|
||||
while (init_indication_controller(get_node()) < 0) {
|
||||
::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
|
||||
::sleep(1);
|
||||
}
|
||||
|
||||
while (init_sim_controller(get_node()) < 0) {
|
||||
::syslog(LOG_INFO, "UAVCAN: sim controller init failed\n");
|
||||
::sleep(1);
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
/*
|
||||
* Set up the time synchronization
|
||||
*/
|
||||
|
||||
const int slave_init_res = _time_sync_slave.start();
|
||||
|
||||
if (slave_init_res < 0) {
|
||||
warnx("Failed to start time_sync_slave");
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
const unsigned PollTimeoutMs = 50;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/* If we had an RTC we would call uavcan_stm32::clock::setUtc()
|
||||
* but for now we use adjustUtc with a correction of 0
|
||||
*/
|
||||
// uavcan_stm32::clock::adjustUtc(uavcan::UtcDuration::fromUSec(0));
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
/*
|
||||
* 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).
|
||||
*/
|
||||
add_poll_fd(busevent_fd);
|
||||
|
||||
uint32_t start_tick = clock_systimer();
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// Mutex is unlocked while the thread is blocked on IO multiplexing
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
|
||||
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
|
||||
// this would be bad...
|
||||
if (poll_ret < 0) {
|
||||
PX4_ERR("poll error %d", errno);
|
||||
continue;
|
||||
|
||||
} else {
|
||||
// Do Something
|
||||
}
|
||||
|
||||
if (clock_systimer() - start_tick > TICK_PER_SEC) {
|
||||
start_tick = clock_systimer();
|
||||
resources("Udate:");
|
||||
/*
|
||||
* Printing the slave status information once a second
|
||||
*/
|
||||
const bool active = _time_sync_slave.isActive(); // Whether it can sync with a remote master
|
||||
|
||||
const int master_node_id = _time_sync_slave.getMasterNodeID().get(); // Returns an invalid Node ID if (active == false)
|
||||
|
||||
const long msec_since_last_adjustment = (_node.getMonotonicTime() - _time_sync_slave.getLastAdjustmentTime()).toMSec();
|
||||
const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
|
||||
syslog(LOG_INFO, "Time:%lld\n"
|
||||
" Time sync slave status:\n"
|
||||
" Active: %d\n"
|
||||
" Master Node ID: %d\n"
|
||||
" Last clock adjustment was %ld ms ago\n",
|
||||
utc .toUSec(), int(active), master_node_id, msec_since_last_adjustment);
|
||||
syslog(LOG_INFO, "UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n\n",
|
||||
static_cast<unsigned long>(utc.toMSec() / 1000),
|
||||
static_cast<double>(uavcan_stm32::clock::getUtcRateCorrectionPPM()),
|
||||
uavcan_stm32::clock::getUtcJumpCount(),
|
||||
int(uavcan_stm32::clock::isUtcLocked()));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
teardown();
|
||||
warnx("exiting.");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
UavcanNode::teardown()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
lock();
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
/*
|
||||
* App entry point
|
||||
*/
|
||||
static void print_usage()
|
||||
{
|
||||
warnx("usage: \n"
|
||||
"\tuavcannode {start|status|stop|arm|disarm}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_start(int argc, char *argv[]);
|
||||
|
||||
int uavcannode_start(int argc, char *argv[])
|
||||
{
|
||||
resources("Before board_app_initialize");
|
||||
|
||||
board_app_initialize(NULL);
|
||||
|
||||
resources("After board_app_initialize");
|
||||
|
||||
// CAN bitrate
|
||||
int32_t bitrate = 0;
|
||||
// Node ID
|
||||
int32_t node_id = 0;
|
||||
|
||||
// Did the bootloader auto baud and get a node ID Allocated
|
||||
|
||||
bootloader_app_shared_t shared;
|
||||
int valid = bootloader_app_shared_read(&shared, BootLoader);
|
||||
|
||||
if (valid == 0) {
|
||||
|
||||
bitrate = shared.bus_speed;
|
||||
node_id = shared.node_id;
|
||||
|
||||
// Invalidate to prevent deja vu
|
||||
|
||||
bootloader_app_shared_invalidate();
|
||||
|
||||
} else {
|
||||
|
||||
// Node ID
|
||||
(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
|
||||
(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
|
||||
}
|
||||
|
||||
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
|
||||
warnx("Invalid Node ID %i", node_id);
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
// Start
|
||||
warnx("Node ID %u, bitrate %u", node_id, bitrate);
|
||||
int rv = UavcanNode::start(node_id, bitrate);
|
||||
resources("After UavcanNode::start");
|
||||
::sleep(1);
|
||||
return rv;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]);
|
||||
int uavcannode_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "start")) {
|
||||
|
||||
if (UavcanNode::instance()) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
return uavcannode_start(argc, argv);
|
||||
}
|
||||
|
||||
/* commands below require the app to be started */
|
||||
UavcanNode *const inst = UavcanNode::instance();
|
||||
|
||||
if (!inst) {
|
||||
errx(1, "application not running");
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
|
||||
inst->print_info();
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "stop")) {
|
||||
delete inst;
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
@@ -1,153 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* 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 <px4_config.h>
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#include <drivers/device/device.h>
|
||||
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
||||
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
|
||||
#include <uavcan/node/timer.hpp>
|
||||
/**
|
||||
* @file uavcan_main.hpp
|
||||
*
|
||||
* Defines basic functionality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/node"
|
||||
|
||||
// we add two to allow for actuator_direct and busevent
|
||||
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
|
||||
|
||||
/**
|
||||
* A UAVCAN node.
|
||||
*/
|
||||
class UavcanNode : public cdev::CDev
|
||||
{
|
||||
/*
|
||||
* This memory is reserved for uavcan to use as over flow for message
|
||||
* Coming from multiple sources that my not be considered at development
|
||||
* time.
|
||||
*
|
||||
* The call to getNumFreeBlocks will tell how many blocks there are
|
||||
* free -and multiply it times getBlockSize to get the number of bytes
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned MemPoolSize = 2048;
|
||||
|
||||
/*
|
||||
* This memory is reserved for uavcan to use for queuing CAN frames.
|
||||
* At 1Mbit there is approximately one CAN frame every 200 uS.
|
||||
* The number of buffers sets how long you can go without calling
|
||||
* node_spin_xxxx. Since our task is the only one running and the
|
||||
* driver will light the fd when there is a CAN frame we can nun with
|
||||
* a minimum number of buffers to conserver memory. Each buffer is
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a maximum required
|
||||
* poll rate of ~1 mS
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned RxQueueLenPerIface = 5;
|
||||
|
||||
/*
|
||||
* This memory is uses for the tasks stack size
|
||||
*/
|
||||
|
||||
static constexpr unsigned StackSize = 2500;
|
||||
|
||||
public:
|
||||
typedef uavcan::Node<MemPoolSize> Node;
|
||||
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;
|
||||
|
||||
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 &get_node() { return _node; }
|
||||
|
||||
int teardown();
|
||||
|
||||
void print_info();
|
||||
|
||||
static UavcanNode *instance() { return _instance; }
|
||||
|
||||
|
||||
/* The bit rate that can be passed back to the bootloader */
|
||||
|
||||
int32_t active_bitrate;
|
||||
|
||||
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
int add_poll_fd(int fd); ///< add a fd to poll list, returning index into _poll_fds[]
|
||||
|
||||
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
|
||||
pollfd _poll_fds[UAVCAN_NUM_POLL_FDS] = {};
|
||||
unsigned _poll_fds_num = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
void (UavcanNode::*)(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &)>
|
||||
BeginFirmwareUpdateCallBack;
|
||||
|
||||
uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateCallBack> _fw_update_listner;
|
||||
void cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp);
|
||||
|
||||
public:
|
||||
|
||||
/* A timer used to reboot after the response is sent */
|
||||
|
||||
uavcan::TimerEventForwarder<void (*)(const uavcan::TimerEvent &)> _reset_timer;
|
||||
|
||||
};
|
||||
@@ -1,59 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <px4_config.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
/**
|
||||
* UAVCAN Node ID.
|
||||
*
|
||||
* Read the specs at http://uavcan.org to learn more about Node ID.
|
||||
*
|
||||
* @min 1
|
||||
* @max 125
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CANNODE_NODE_ID, 120);
|
||||
|
||||
/**
|
||||
* UAVCAN CAN bus bitrate.
|
||||
*
|
||||
* @min 20000
|
||||
* @max 1000000
|
||||
* @group UAVCAN
|
||||
*/
|
||||
PARAM_DEFINE_INT32(CANNODE_BITRATE, 1000000);
|
||||
Reference in New Issue
Block a user