mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
Changes to build on latest uavcan master with FW upload and Node ID allocation
This commit is contained in:
David Sidrane
parent
3dbd48fbad
commit
d720a42a35
@ -1 +1 @@
|
||||
Subproject commit 7719f7692aba67f01b6321773bb7be13f23d2f68
|
||||
Subproject commit 1f1679c75d989420350e69339d48b53203c5e874
|
||||
@ -39,6 +39,8 @@
|
||||
MODULE_COMMAND = uavcan
|
||||
|
||||
MAXOPTIMIZATION = -O3
|
||||
MODULE_STACKSIZE = 3200
|
||||
WFRAME_LARGER_THAN = 1400
|
||||
|
||||
# Main
|
||||
SRCS += uavcan_main.cpp \
|
||||
@ -65,7 +67,6 @@ INCLUDE_DIRS += $(LIBUAVCAN_INC)
|
||||
# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
|
||||
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
|
||||
|
||||
|
||||
#
|
||||
# libuavcan drivers for STM32
|
||||
#
|
||||
@ -75,6 +76,12 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC))
|
||||
INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
|
||||
override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
|
||||
|
||||
#
|
||||
# libuavcan drivers for posix
|
||||
#
|
||||
include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/posix/include.mk
|
||||
INCLUDE_DIRS += $(LIBUAVCAN_POSIX_INC)
|
||||
|
||||
#
|
||||
# Invoke DSDL compiler
|
||||
#
|
||||
|
||||
@ -42,9 +42,11 @@ 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_data(node),
|
||||
_sub_air_pressure_data(node),
|
||||
_sub_air_temperature_data(node),
|
||||
_reports(nullptr)
|
||||
{
|
||||
last_temperature = 0.0f;
|
||||
}
|
||||
|
||||
int UavcanBarometerBridge::init()
|
||||
@ -59,11 +61,17 @@ int UavcanBarometerBridge::init()
|
||||
if (_reports == nullptr)
|
||||
return -1;
|
||||
|
||||
res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
|
||||
if (res < 0) {
|
||||
log("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
|
||||
if (res < 0) {
|
||||
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) {
|
||||
log("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
@ -127,31 +135,36 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
|
||||
void UavcanBarometerBridge::air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
|
||||
{
|
||||
baro_report report;
|
||||
|
||||
report.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
report.temperature = msg.static_temperature;
|
||||
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
|
||||
report.error_count = 0;
|
||||
|
||||
/*
|
||||
* Altitude computation
|
||||
* Refer to the MS5611 driver for details
|
||||
*/
|
||||
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
|
||||
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
|
||||
const double g = 9.80665; // gravity constant in m/s/s
|
||||
const double R = 287.05; // ideal gas constant in J/kg/K
|
||||
|
||||
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
|
||||
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
|
||||
|
||||
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
|
||||
|
||||
// add to the ring buffer
|
||||
_reports->force(&report);
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &report);
|
||||
last_temperature = msg.static_temperature;
|
||||
}
|
||||
|
||||
void UavcanBarometerBridge::air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
|
||||
{
|
||||
baro_report report;
|
||||
|
||||
report.timestamp = msg.getMonotonicTimestamp().toUSec();
|
||||
report.temperature = last_temperature;
|
||||
report.pressure = msg.static_pressure / 100.0F; // Convert to millibar
|
||||
report.error_count = 0;
|
||||
|
||||
/*
|
||||
* Altitude computation
|
||||
* Refer to the MS5611 driver for details
|
||||
*/
|
||||
const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin
|
||||
const double a = -6.5 / 1000; // temperature gradient in degrees per metre
|
||||
const double g = 9.80665; // gravity constant in m/s/s
|
||||
const double R = 287.05; // ideal gas constant in J/kg/K
|
||||
|
||||
const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa
|
||||
const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa
|
||||
|
||||
report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a;
|
||||
|
||||
// add to the ring buffer
|
||||
_reports->force(&report);
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &report);
|
||||
}
|
||||
|
||||
@ -41,7 +41,10 @@
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
|
||||
#include <uavcan/equipment/air_data/StaticAirData.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticPressure.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||
|
||||
class RingBuffer;
|
||||
|
||||
class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
@ -58,14 +61,23 @@ 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_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
|
||||
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::StaticAirData> &) >
|
||||
AirDataCbBinder;
|
||||
typedef uavcan::MethodBinder < UavcanBarometerBridge *,
|
||||
void (UavcanBarometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
|
||||
AirPressureCbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
|
||||
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;
|
||||
unsigned _msl_pressure = 101325;
|
||||
ringbuffer::RingBuffer *_reports;
|
||||
float last_temperature;
|
||||
|
||||
};
|
||||
|
||||
@ -53,25 +53,42 @@
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "uavcan_main.hpp"
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
|
||||
#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
|
||||
|
||||
#include <uavcan_posix/firmware_version_checker.hpp>
|
||||
|
||||
//todo:The Inclusion of file_server_backend is killing
|
||||
// #include <sys/types.h> and leaving OK undefined
|
||||
#define OK 0
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
*
|
||||
* Implements basic functinality of UAVCAN node.
|
||||
* Implements basic functionality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
/*
|
||||
* UavcanNode
|
||||
*/
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
|
||||
uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
|
||||
uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
|
||||
uavcan_posix::FirmwareVersionChecker fw_version_checker;
|
||||
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev("uavcan", UAVCAN_DEVICE_PATH),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
_esc_controller(_node)
|
||||
_esc_controller(_node),
|
||||
_fileserver_backend(_node),
|
||||
_node_info_retriever(_node),
|
||||
_fw_upgrade_trigger(_node, fw_version_checker),
|
||||
_fw_server(_node, _fileserver_backend)
|
||||
{
|
||||
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||
@ -79,6 +96,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
||||
_control_topics[3] = ORB_ID(actuator_controls_3);
|
||||
|
||||
const int res = pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
@ -124,6 +142,7 @@ UavcanNode::~UavcanNode()
|
||||
|
||||
// Removing the sensor bridges
|
||||
auto br = _sensor_bridges.getHead();
|
||||
|
||||
while (br != nullptr) {
|
||||
auto next = br->getSibling();
|
||||
delete br;
|
||||
@ -135,6 +154,8 @@ UavcanNode::~UavcanNode()
|
||||
perf_free(_perfcnt_node_spin_elapsed);
|
||||
perf_free(_perfcnt_esc_mixer_output_elapsed);
|
||||
perf_free(_perfcnt_esc_mixer_total_elapsed);
|
||||
delete(_server_instance);
|
||||
|
||||
}
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
@ -196,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
*/
|
||||
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);
|
||||
static_cast<main_t>(run_trampoline), nullptr);
|
||||
|
||||
if (_instance->_task < 0) {
|
||||
warnx("start failed: %d", errno);
|
||||
@ -228,8 +249,10 @@ void UavcanNode::fill_node_info()
|
||||
|
||||
if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
|
||||
hwver.major = 1;
|
||||
|
||||
} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
|
||||
hwver.major = 2;
|
||||
|
||||
} else {
|
||||
; // All other values of HW_ARCH resolve to zero
|
||||
}
|
||||
@ -241,6 +264,7 @@ void UavcanNode::fill_node_info()
|
||||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id)
|
||||
{
|
||||
int ret = -1;
|
||||
@ -260,6 +284,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
||||
|
||||
// Actuators
|
||||
ret = _esc_controller.init();
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
@ -267,26 +292,101 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
||||
// Sensor bridges
|
||||
IUavcanSensorBridge::make_all(_node, _sensor_bridges);
|
||||
auto br = _sensor_bridges.getHead();
|
||||
|
||||
while (br != nullptr) {
|
||||
ret = br->init();
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
warnx("sensor bridge '%s' init ok", br->get_name());
|
||||
br = br->getSibling();
|
||||
}
|
||||
|
||||
|
||||
/* Initialize the fw version checker.
|
||||
* giving it it's path
|
||||
*/
|
||||
|
||||
ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/* Start fw file server back */
|
||||
|
||||
ret = _fw_server.start();
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
|
||||
|
||||
ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
|
||||
|
||||
ret = tracer.init(UAVCAN_LOG_FILE);
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Create dynamic node id server for the Firmware updates directory */
|
||||
|
||||
_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
|
||||
|
||||
if (_server_instance == 0) {
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* Initialize the dynamic node id server */
|
||||
ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start node info retriever to fetch node info from new nodes */
|
||||
|
||||
ret = _node_info_retriever.start();
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
/* Start the fw version checker */
|
||||
|
||||
ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
|
||||
|
||||
if (ret < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start the Node */
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
void UavcanNode::node_spin_once()
|
||||
{
|
||||
perf_begin(_perfcnt_node_spin_elapsed);
|
||||
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||
const int spin_res = _node.spinOnce();
|
||||
|
||||
if (spin_res < 0) {
|
||||
warnx("node spin error %i", spin_res);
|
||||
}
|
||||
|
||||
perf_end(_perfcnt_node_spin_elapsed);
|
||||
}
|
||||
|
||||
@ -297,9 +397,11 @@ void UavcanNode::node_spin_once()
|
||||
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;
|
||||
@ -312,8 +414,6 @@ int UavcanNode::run()
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
const unsigned PollTimeoutMs = 50;
|
||||
|
||||
// XXX figure out the output count
|
||||
_output_count = 2;
|
||||
|
||||
@ -324,6 +424,7 @@ int UavcanNode::run()
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
|
||||
const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
|
||||
|
||||
if (busevent_fd < 0)
|
||||
{
|
||||
warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
|
||||
@ -354,6 +455,7 @@ int UavcanNode::run()
|
||||
}
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
// update actuator controls subscriptions if needed
|
||||
if (_groups_subscribed != _groups_required) {
|
||||
subscribe();
|
||||
@ -379,9 +481,11 @@ int UavcanNode::run()
|
||||
if (poll_ret < 0) {
|
||||
log("poll error %d", errno);
|
||||
continue;
|
||||
|
||||
} else {
|
||||
// get controls for required topics
|
||||
bool controls_updated = false;
|
||||
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (_control_subs[i] > 0) {
|
||||
if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
|
||||
@ -401,8 +505,9 @@ int UavcanNode::run()
|
||||
if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
|
||||
_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
|
||||
}
|
||||
|
||||
memcpy(&_outputs.output[0], &_actuator_direct.values[0],
|
||||
_actuator_direct.nvalues*sizeof(float));
|
||||
_actuator_direct.nvalues * sizeof(float));
|
||||
_outputs.noutputs = _actuator_direct.nvalues;
|
||||
new_output = true;
|
||||
}
|
||||
@ -411,10 +516,12 @@ int UavcanNode::run()
|
||||
if (_test_in_progress) {
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
|
||||
_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
|
||||
_outputs.noutputs = _test_motor.motor_number+1;
|
||||
_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
|
||||
_outputs.noutputs = _test_motor.motor_number + 1;
|
||||
}
|
||||
|
||||
new_output = true;
|
||||
|
||||
} else if (controls_updated && (_mixers != nullptr)) {
|
||||
|
||||
// XXX one output group has 8 outputs max,
|
||||
@ -451,6 +558,7 @@ int UavcanNode::run()
|
||||
_outputs.output[i] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// Output to the bus
|
||||
_outputs.timestamp = hrt_absolute_time();
|
||||
perf_begin(_perfcnt_esc_mixer_output_elapsed);
|
||||
@ -509,6 +617,7 @@ UavcanNode::teardown()
|
||||
_control_subs[i] = -1;
|
||||
}
|
||||
}
|
||||
|
||||
return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
|
||||
}
|
||||
|
||||
@ -526,12 +635,14 @@ UavcanNode::subscribe()
|
||||
// Subscribe/unsubscribe to required actuator control groups
|
||||
uint32_t sub_groups = _groups_required & ~_groups_subscribed;
|
||||
uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
|
||||
|
||||
// the first fd used by CAN
|
||||
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
if (sub_groups & (1 << i)) {
|
||||
warnx("subscribe to actuator_controls_%d", i);
|
||||
_control_subs[i] = orb_subscribe(_control_topics[i]);
|
||||
}
|
||||
|
||||
if (unsub_groups & (1 << i)) {
|
||||
warnx("unsubscribe from actuator_controls_%d", i);
|
||||
::close(_control_subs[i]);
|
||||
@ -583,8 +694,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
const char *buf = (const char *)arg;
|
||||
unsigned buflen = strnlen(buf, 1024);
|
||||
|
||||
if (_mixers == nullptr)
|
||||
if (_mixers == nullptr) {
|
||||
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
|
||||
}
|
||||
|
||||
if (_mixers == nullptr) {
|
||||
_groups_required = 0;
|
||||
@ -600,6 +712,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
_mixers = nullptr;
|
||||
_groups_required = 0;
|
||||
ret = -EINVAL;
|
||||
|
||||
} else {
|
||||
|
||||
_mixers->groups_required(_groups_required);
|
||||
@ -640,9 +753,10 @@ UavcanNode::print_info()
|
||||
if (_outputs.noutputs != 0) {
|
||||
printf("ESC output: ");
|
||||
|
||||
for (uint8_t i=0; i<_outputs.noutputs; i++) {
|
||||
printf("%d ", (int)(_outputs.output[i]*1000));
|
||||
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
|
||||
printf("%d ", (int)(_outputs.output[i] * 1000));
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
// ESC status
|
||||
@ -653,7 +767,8 @@ UavcanNode::print_info()
|
||||
|
||||
printf("ESC Status:\n");
|
||||
printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
|
||||
for (uint8_t i=0; i<_outputs.noutputs; i++) {
|
||||
|
||||
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
|
||||
printf("%d\t", esc.esc[i].esc_address);
|
||||
printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
|
||||
printf("%3.2f\t", (double)esc.esc[i].esc_current);
|
||||
@ -669,6 +784,7 @@ UavcanNode::print_info()
|
||||
|
||||
// Sensor bridges
|
||||
auto br = _sensor_bridges.getHead();
|
||||
|
||||
while (br != nullptr) {
|
||||
printf("Sensor '%s':\n", br->get_name());
|
||||
br->print_status();
|
||||
|
||||
@ -47,6 +47,14 @@
|
||||
#include "actuators/esc.hpp"
|
||||
#include "sensors/sensor_bridge.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>
|
||||
|
||||
|
||||
/**
|
||||
* @file uavcan_main.hpp
|
||||
*
|
||||
@ -57,18 +65,40 @@
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/esc"
|
||||
#define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db"
|
||||
#define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw"
|
||||
#define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log"
|
||||
|
||||
// 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 device::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 = 10;
|
||||
|
||||
static constexpr unsigned MemPoolSize = 10752; ///< Refer to the libuavcan manual to learn why
|
||||
static constexpr unsigned RxQueueLenPerIface = 64;
|
||||
static constexpr unsigned StackSize = 3000;
|
||||
|
||||
/*
|
||||
* 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 = 3400;
|
||||
|
||||
public:
|
||||
typedef uavcan::Node<MemPoolSize> Node;
|
||||
@ -117,11 +147,19 @@ private:
|
||||
unsigned _output_count = 0; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer
|
||||
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
UavcanEscController _esc_controller;
|
||||
|
||||
|
||||
uavcan_posix::BasicFileSeverBackend _fileserver_backend;
|
||||
uavcan::NodeInfoRetriever _node_info_retriever;
|
||||
uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger;
|
||||
uavcan::BasicFileServer _fw_server;
|
||||
|
||||
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
|
||||
|
||||
MixerGroup *_mixers = nullptr;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user