drivers/uavcan: make firmware server available on both CAN1/CAN2 always (if enabled)

- uavcan firmware server no longer shuts down when arming (nodes might restart in flight)
 - always handle UAVCAN parameters with or without the FW server active
 - remove legacy ESC enumeration in FW server
This commit is contained in:
Daniel Agar
2022-01-10 11:13:02 -05:00
committed by GitHub
parent d9e7315420
commit 2e2ac36cab
7 changed files with 565 additions and 1822 deletions
+2 -8
View File
@@ -291,15 +291,9 @@ else
# Start core UAVCAN module.
if uavcan start
then
if param greater UAVCAN_ENABLE 1
if param greater UAVCAN_ENABLE 2
then
# Start UAVCAN firmware update server and dynamic node ID allocation server.
uavcan start fw
if param greater UAVCAN_ENABLE 2
then
set OUTPUT_MODE uavcan_esc
fi
set OUTPUT_MODE uavcan_esc
fi
else
tune_control play error
File diff suppressed because it is too large Load Diff
+79 -37
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2021 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
@@ -46,32 +46,37 @@
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include "beep.hpp"
#include "rgbled.hpp"
#include "safety_state.hpp"
#include "uavcan_driver.hpp"
#include "uavcan_servers.hpp"
#include "allocator.hpp"
#include "actuators/esc.hpp"
#include "actuators/hardpoint.hpp"
#include "actuators/servo.hpp"
#include "allocator.hpp"
#include "beep.hpp"
#include "rgbled.hpp"
#include "safety_state.hpp"
#include "sensors/sensor_bridge.hpp"
#include "uavcan_driver.hpp"
#include "uavcan_servers.hpp"
#include <lib/drivers/device/Device.hpp>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#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_info_retriever.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
#include <uavcan/protocol/param/GetSet.hpp>
#include <uavcan/protocol/RestartNode.hpp>
#include <lib/drivers/device/device.h>
#include <lib/mixer_module/mixer_module.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/vehicle_command_ack.h>
using namespace time_literals;
@@ -178,18 +183,15 @@ public:
uavcan::Node<> &get_node() { return _node; }
int teardown();
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;}
void requestCheckAllNodesFirmwareAndUpdate() { _check_fw = true; }
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);
@@ -198,16 +200,11 @@ public:
static void busevent_signal_trampoline();
protected:
void Run() override;
private:
void Run() override;
void fill_node_info();
int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
void node_spin_once();
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);
@@ -219,10 +216,6 @@ private:
void enable_idle_throttle_when_armed(bool value);
px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
px4::atomic<int> _fw_server_action{None};
int _fw_server_status{-1};
bool _is_armed{false}; ///< the arming status of the actuators on the bus
unsigned _output_count{0}; ///< number of actuators currently available
@@ -232,30 +225,28 @@ private:
uavcan::Node<> _node; ///< library instance
pthread_mutex_t _node_mutex;
px4_sem_t _server_command_sem;
UavcanEscController _esc_controller;
UavcanServoController _servo_controller;
UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller};
UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller};
UavcanHardpointController _hardpoint_controller;
UavcanBeep _beep_controller;
UavcanSafetyState _safety_state_controller;
UavcanRGBController _rgbled_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
uavcan::NodeInfoRetriever _node_info_retriever;
ITxQueueInjector *_tx_injector{nullptr};
List<IUavcanSensorBridge *> _sensor_bridges; ///< List of active sensor bridges
bool _idle_throttle_when_armed{false};
int32_t _idle_throttle_when_armed_param{0};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};
void handle_time_sync(const uavcan::TimerEvent &);
@@ -274,7 +265,58 @@ private:
void (UavcanNode::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
void cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)};
uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)};
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
/*
* 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};
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); }
bool _check_fw{false};
UavcanServers *_servers{nullptr};
};
-12
View File
@@ -66,18 +66,6 @@
// 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[]);
File diff suppressed because it is too large Load Diff
+16 -154
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2014-2021 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
@@ -34,33 +34,18 @@
#pragma once
#include <px4_platform_common/px4_config.h>
#include "uavcan_driver.hpp"
#include <drivers/device/Device.hpp>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/vehicle_command_ack.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/protocol/firmware_update_trigger.hpp>
#include <uavcan/protocol/node_info_retriever.hpp>
#include <uavcan/protocol/node_status_monitor.hpp>
#include <uavcan_posix/basic_file_server_backend.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
@@ -74,152 +59,29 @@
/**
* A UAVCAN Server Sub node.
*/
class __EXPORT UavcanServers
class 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);
UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
~UavcanServers() = default;
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; }
int init();
bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
private:
pthread_t _subnode_thread{-1};
pthread_mutex_t _subnode_mutex{};
px4::atomic_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;
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
int copyFw(const char *dst, const char *src);
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;
uavcan_posix::BasicFileServerBackend _fileserver_backend;
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
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
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);
void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
int copyFw(const char *dst, const char *src);
uavcan::NodeInfoRetriever &_node_info_retriever;
};
File diff suppressed because it is too large Load Diff