forked from Archive/PX4-Autopilot
uavcan module cleanup
- move most orb to uORB::Publication and uORB::Subscription - update legacy message handling (warn to PX4_INFO, PX4_WARN, PX4_ERR) - add perf counters - sensors/mag support newer `uavcan::equipment::ahrs::MagneticFieldStrength2` message - sensors/gps support `uavcan::equipment::gnss::Auxiliary` for hdop and vdop - sensors delete obsolete ioctl and read methods - use PublicationMulti for actuator_outputs and esc_reports (to coexist with other output modules) - add GNSS parameter metadata (parameters_injected.xml)
This commit is contained in:
Daniel Agar
GitHub
parent
47dd312b57
commit
375fc4a75c
|
|
@ -52,14 +52,6 @@ UavcanEscController::UavcanEscController(uavcan::INode &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()
|
||||
|
|
@ -68,13 +60,14 @@ UavcanEscController::~UavcanEscController()
|
|||
perf_free(_perfcnt_scaling_error);
|
||||
}
|
||||
|
||||
int UavcanEscController::init()
|
||||
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);
|
||||
PX4_ERR("ESC status sub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
|
|
@ -85,11 +78,13 @@ int UavcanEscController::init()
|
|||
return res;
|
||||
}
|
||||
|
||||
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||
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;
|
||||
}
|
||||
|
|
@ -111,9 +106,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
|||
*/
|
||||
uavcan::equipment::esc::RawCommand msg;
|
||||
|
||||
actuator_outputs_s actuator_outputs = {};
|
||||
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;
|
||||
|
|
@ -167,21 +161,15 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_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);
|
||||
_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);
|
||||
}
|
||||
|
||||
actuator_outputs.timestamp = hrt_absolute_time();
|
||||
_actuator_outputs_pub.publish(actuator_outputs);
|
||||
}
|
||||
|
||||
void UavcanEscController::arm_all_escs(bool arm)
|
||||
void
|
||||
UavcanEscController::arm_all_escs(bool arm)
|
||||
{
|
||||
if (arm) {
|
||||
_armed_mask = -1;
|
||||
|
|
@ -191,7 +179,8 @@ void UavcanEscController::arm_all_escs(bool arm)
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::arm_single_esc(int num, bool arm)
|
||||
void
|
||||
UavcanEscController::arm_single_esc(int num, bool arm)
|
||||
{
|
||||
if (arm) {
|
||||
_armed_mask = MOTOR_BIT(num);
|
||||
|
|
@ -201,7 +190,8 @@ void UavcanEscController::arm_single_esc(int num, bool arm)
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||
void
|
||||
UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
|
||||
{
|
||||
if (msg.esc_index < esc_status_s::CONNECTED_ESC_MAX) {
|
||||
auto &ref = _esc_status.esc[msg.esc_index];
|
||||
|
|
@ -217,7 +207,8 @@ void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
|
||||
void
|
||||
UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
|
||||
{
|
||||
_esc_status.timestamp = hrt_absolute_time();
|
||||
_esc_status.esc_count = _rotor_count;
|
||||
|
|
@ -225,18 +216,14 @@ void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent &)
|
|||
_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
|
||||
_esc_status.esc_online_flags = UavcanEscController::check_escs_status();
|
||||
|
||||
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);
|
||||
}
|
||||
_esc_status_pub.publish(_esc_status);
|
||||
}
|
||||
|
||||
uint8_t UavcanEscController::check_escs_status()
|
||||
uint8_t
|
||||
UavcanEscController::check_escs_status()
|
||||
{
|
||||
int esc_status_flags = 0;
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
|
||||
|
||||
|
|
|
|||
|
|
@ -47,12 +47,12 @@
|
|||
#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 <lib/perf/perf_counter.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
|
||||
class UavcanEscController
|
||||
{
|
||||
public:
|
||||
|
|
@ -95,17 +95,19 @@ private:
|
|||
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;
|
||||
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)> StatusCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanEscController *, void (UavcanEscController::*)(const uavcan::TimerEvent &)>
|
||||
TimerCbBinder;
|
||||
typedef uavcan::MethodBinder<UavcanEscController *,
|
||||
void (UavcanEscController::*)(const uavcan::TimerEvent &)> TimerCbBinder;
|
||||
|
||||
bool _armed{false};
|
||||
bool _run_at_idle_throttle_when_armed{false};
|
||||
|
||||
esc_status_s _esc_status{};
|
||||
|
||||
uORB::PublicationMulti<actuator_outputs_s> _actuator_outputs_pub{ORB_ID(actuator_outputs)};
|
||||
uORB::PublicationMulti<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
|
||||
|
||||
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;
|
||||
uint8_t _rotor_count = 0;
|
||||
|
||||
/*
|
||||
|
|
@ -120,12 +122,12 @@ private:
|
|||
/*
|
||||
* ESC states
|
||||
*/
|
||||
uint32_t _armed_mask = 0;
|
||||
uint8_t _max_number_of_nonzero_outputs = 0;
|
||||
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");
|
||||
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")};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -48,13 +48,13 @@ UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
|
|||
_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
|
||||
}
|
||||
|
||||
|
||||
UavcanHardpointController::~UavcanHardpointController()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
int UavcanHardpointController::init()
|
||||
int
|
||||
UavcanHardpointController::init()
|
||||
{
|
||||
/*
|
||||
* Setup timer and call back function for periodic updates
|
||||
|
|
@ -63,7 +63,8 @@ int UavcanHardpointController::init()
|
|||
return 0;
|
||||
}
|
||||
|
||||
void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
|
||||
void
|
||||
UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
|
||||
{
|
||||
_cmd.command = command;
|
||||
_cmd.hardpoint_id = hardpoint_id;
|
||||
|
|
@ -71,7 +72,7 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
|
|||
/*
|
||||
* Publish the command message to the bus
|
||||
*/
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
|
||||
/*
|
||||
* Start the periodic update timer after a command is set
|
||||
|
|
@ -81,10 +82,10 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma
|
|||
}
|
||||
|
||||
}
|
||||
void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
|
||||
|
||||
void
|
||||
UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
|
||||
{
|
||||
/*
|
||||
* Broadcast command at MAX_RATE_HZ
|
||||
*/
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
// Broadcast command at MAX_RATE_HZ
|
||||
_uavcan_pub_raw_cmd.broadcast(_cmd);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -61,8 +61,8 @@ struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSiz
|
|||
~Allocator()
|
||||
{
|
||||
if (getNumAllocatedBlocks() > 0) {
|
||||
warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
PX4_ERR("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -44,8 +44,7 @@ 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))
|
||||
_sub_air_temperature_data(node)
|
||||
{ }
|
||||
|
||||
int UavcanBarometerBridge::init()
|
||||
|
|
@ -73,49 +72,15 @@ int UavcanBarometerBridge::init()
|
|||
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
|
||||
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
|
||||
void
|
||||
UavcanBarometerBridge::air_pressure_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
|
||||
{
|
||||
sensor_baro_s report{};
|
||||
|
|
@ -135,8 +100,5 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const
|
|||
/* TODO get device ID for sensor */
|
||||
report.device_id = 0;
|
||||
|
||||
// add to the ring buffer
|
||||
_reports.force(&report);
|
||||
|
||||
publish(msg.getSrcNodeID().get(), &report);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -39,13 +39,10 @@
|
|||
|
||||
#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:
|
||||
|
|
@ -58,8 +55,6 @@ public:
|
|||
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);
|
||||
|
|
@ -77,8 +72,6 @@ private:
|
|||
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;
|
||||
float last_temperature_kelvin{0.0f};
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -47,24 +47,22 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <mathlib/mathlib.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
const char *const UavcanGnssBridge::NAME = "gnss";
|
||||
|
||||
UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) :
|
||||
_node(node),
|
||||
_sub_auxiliary(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)
|
||||
_orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb))
|
||||
{
|
||||
}
|
||||
|
||||
UavcanGnssBridge::~UavcanGnssBridge()
|
||||
{
|
||||
(void) orb_unsubscribe(_orb_sub_gnss);
|
||||
}
|
||||
|
||||
int UavcanGnssBridge::init()
|
||||
int
|
||||
UavcanGnssBridge::init()
|
||||
{
|
||||
int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
|
||||
|
||||
|
|
@ -73,6 +71,13 @@ int UavcanGnssBridge::init()
|
|||
return res;
|
||||
}
|
||||
|
||||
res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS auxiliary sub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
|
|
@ -87,18 +92,19 @@ int UavcanGnssBridge::init()
|
|||
return res;
|
||||
}
|
||||
|
||||
_orb_to_uavcan_pub_timer.startPeriodic(
|
||||
uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
|
||||
_orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ));
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
unsigned UavcanGnssBridge::get_num_redundant_channels() const
|
||||
unsigned
|
||||
UavcanGnssBridge::get_num_redundant_channels() const
|
||||
{
|
||||
return (_receiver_node_id < 0) ? 0 : 1;
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::print_status() const
|
||||
void
|
||||
UavcanGnssBridge::print_status() const
|
||||
{
|
||||
printf("RX errors: %d, using old Fix: %d, receiver node id: ",
|
||||
_sub_fix.getFailureCount(), int(_old_fix_subscriber_active));
|
||||
|
|
@ -111,7 +117,17 @@ void UavcanGnssBridge::print_status() const
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
|
||||
void
|
||||
UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg)
|
||||
{
|
||||
// store latest hdop and vdop for use in process_fixx();
|
||||
_last_gnss_auxiliary_timestamp = hrt_absolute_time();
|
||||
_last_gnss_auxiliary_hdop = msg.hdop;
|
||||
_last_gnss_auxiliary_vdop = msg.vdop;
|
||||
}
|
||||
|
||||
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();
|
||||
|
|
@ -125,10 +141,11 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
|
|||
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)
|
||||
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");
|
||||
PX4_INFO("GNSS Fix2 message detected, disabling support for the old Fix message");
|
||||
_sub_fix.stop();
|
||||
_old_fix_subscriber_active = false;
|
||||
_receiver_node_id = -1;
|
||||
|
|
@ -139,8 +156,8 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
bool valid_covariances = true;
|
||||
|
||||
switch (msg.covariance.size()) {
|
||||
// Scalar matrix
|
||||
case 1: {
|
||||
// Scalar matrix
|
||||
const auto x = msg.covariance[0];
|
||||
|
||||
pos_cov[0] = x;
|
||||
|
|
@ -150,11 +167,11 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
vel_cov[0] = x;
|
||||
vel_cov[4] = x;
|
||||
vel_cov[8] = x;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
// Diagonal matrix (the most common case)
|
||||
case 6: {
|
||||
// Diagonal matrix (the most common case)
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[4] = msg.covariance[1];
|
||||
pos_cov[8] = msg.covariance[2];
|
||||
|
|
@ -162,19 +179,21 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
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
|
||||
}
|
||||
break;
|
||||
|
||||
|
||||
case 21: {
|
||||
// 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
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
|
|
@ -197,17 +216,16 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
}
|
||||
|
||||
/* 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: {
|
||||
// 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
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
|
|
@ -230,9 +248,8 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
}
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
||||
// Either empty or invalid sized, interpret as zero matrix
|
||||
default: {
|
||||
// Either empty or invalid sized, interpret as zero matrix
|
||||
valid_covariances = false;
|
||||
break; // Nothing to do
|
||||
}
|
||||
|
|
@ -243,15 +260,13 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
|||
|
||||
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)
|
||||
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);
|
||||
PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id);
|
||||
|
||||
} else {
|
||||
if (_receiver_node_id != msg.getSrcNodeID().get()) {
|
||||
|
|
@ -259,7 +274,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
|||
}
|
||||
}
|
||||
|
||||
auto report = ::vehicle_gps_position_s();
|
||||
vehicle_gps_position_s report{};
|
||||
|
||||
/*
|
||||
* FIXME HACK
|
||||
|
|
@ -329,107 +344,78 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
|||
|
||||
report.timestamp_time_relative = 0;
|
||||
|
||||
const std::uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();
|
||||
const 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_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;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
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));
|
||||
timespec 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;
|
||||
if (hrt_elapsed_time(&_last_gnss_auxiliary_timestamp) < 2_s) {
|
||||
report.hdop = _last_gnss_auxiliary_hdop;
|
||||
report.vdop = _last_gnss_auxiliary_vdop;
|
||||
|
||||
} else {
|
||||
// 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);
|
||||
_gps_pub.publish(report);
|
||||
|
||||
// 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");
|
||||
PX4_INFO("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers");
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
|
||||
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));
|
||||
vehicle_gps_position_s orb_msg{};
|
||||
|
||||
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);
|
||||
if (!_orb_sub_gnss.update(&orb_msg)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -462,5 +448,5 @@ void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
|
|||
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);
|
||||
_pub_fix2.broadcast(msg);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2014-2019 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
|
||||
|
|
@ -44,10 +44,12 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
||||
#include <uavcan/equipment/gnss/Fix.hpp>
|
||||
#include <uavcan/equipment/gnss/Fix2.hpp>
|
||||
|
||||
|
|
@ -61,7 +63,7 @@ public:
|
|||
static const char *const NAME;
|
||||
|
||||
UavcanGnssBridge(uavcan::INode &node);
|
||||
~UavcanGnssBridge();
|
||||
~UavcanGnssBridge() = default;
|
||||
|
||||
const char *get_name() const override { return NAME; }
|
||||
|
||||
|
|
@ -75,18 +77,21 @@ private:
|
|||
/**
|
||||
* GNSS fix message will be reported via this callback.
|
||||
*/
|
||||
void gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Auxiliary> &msg);
|
||||
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);
|
||||
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::Auxiliary> &) >
|
||||
AuxiliaryCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanGnssBridge *,
|
||||
void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &) >
|
||||
FixCbBinder;
|
||||
|
|
@ -100,17 +105,24 @@ private:
|
|||
TimerCbBinder;
|
||||
|
||||
uavcan::INode &_node;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary, AuxiliaryCbBinder> _sub_auxiliary;
|
||||
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;
|
||||
uint64_t _last_gnss_auxiliary_timestamp{0};
|
||||
float _last_gnss_auxiliary_hdop{0.0f};
|
||||
float _last_gnss_auxiliary_vdop{0.0f};
|
||||
|
||||
orb_advert_t _report_pub; ///< uORB pub for gnss position
|
||||
uORB::PublicationMulti<vehicle_gps_position_s> _gps_pub{ORB_ID(vehicle_gps_position), ORB_PRIO_DEFAULT};
|
||||
uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)};
|
||||
|
||||
int _orb_sub_gnss = -1; ///< uORB sub for gnss position, used for bridging uORB --> UAVCAN
|
||||
int _receiver_node_id{-1};
|
||||
bool _old_fix_subscriber_active{true};
|
||||
bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data?
|
||||
|
||||
bool _system_clock_set = false; ///< Have we set the system clock at least once from GNSS data?
|
||||
};
|
||||
|
|
|
|||
|
|
@ -44,7 +44,8 @@ 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)
|
||||
_sub_mag(node),
|
||||
_sub_mag2(node)
|
||||
{
|
||||
_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; // <-- Why?
|
||||
|
||||
|
|
@ -53,7 +54,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode &node) :
|
|||
_scale.z_scale = 1.0F;
|
||||
}
|
||||
|
||||
int UavcanMagnetometerBridge::init()
|
||||
int
|
||||
UavcanMagnetometerBridge::init()
|
||||
{
|
||||
int res = device::CDev::init();
|
||||
|
||||
|
|
@ -64,40 +66,22 @@ int UavcanMagnetometerBridge::init()
|
|||
res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
DEVICE_LOG("failed to start uavcan sub: %d", res);
|
||||
PX4_ERR("failed to start uavcan sub: %d", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_mag2.start(Mag2CbBinder(this, &UavcanMagnetometerBridge::mag2_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_ERR("failed to start uavcan sub2: %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)
|
||||
int
|
||||
UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
|
|
@ -127,9 +111,33 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
|
||||
&msg)
|
||||
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);
|
||||
}
|
||||
|
||||
void
|
||||
UavcanMagnetometerBridge::mag2_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg)
|
||||
{
|
||||
lock();
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -38,9 +38,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "sensor_bridge.hpp"
|
||||
|
||||
#include <drivers/drv_mag.h>
|
||||
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength.hpp>
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
|
||||
|
||||
class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase
|
||||
{
|
||||
|
|
@ -54,17 +56,25 @@ public:
|
|||
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);
|
||||
void mag2_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &msg);
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
|
||||
void (UavcanMagnetometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength> &) >
|
||||
MagCbBinder;
|
||||
|
||||
typedef uavcan::MethodBinder < UavcanMagnetometerBridge *,
|
||||
void (UavcanMagnetometerBridge::*)
|
||||
(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength2> &) >
|
||||
Mag2CbBinder;
|
||||
|
||||
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength, MagCbBinder> _sub_mag;
|
||||
struct mag_calibration_s _scale = {};
|
||||
mag_report _report = {};
|
||||
uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2, Mag2CbBinder> _sub_mag2;
|
||||
|
||||
mag_calibration_s _scale{};
|
||||
sensor_mag_s _report{};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -70,10 +70,9 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase()
|
|||
delete [] _channels;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
void
|
||||
UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
{
|
||||
assert(report != nullptr);
|
||||
|
||||
Channel *channel = nullptr;
|
||||
|
||||
// Checking if such channel already exists
|
||||
|
|
@ -141,7 +140,8 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
|||
(void)orb_publish(_orb_topic, channel->orb_advert, report);
|
||||
}
|
||||
|
||||
unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
|
||||
unsigned
|
||||
UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
|
||||
{
|
||||
unsigned out = 0;
|
||||
|
||||
|
|
@ -154,7 +154,8 @@ unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const
|
|||
return out;
|
||||
}
|
||||
|
||||
void UavcanCDevSensorBridgeBase::print_status() const
|
||||
void
|
||||
UavcanCDevSensorBridgeBase::print_status() const
|
||||
{
|
||||
printf("devname: %s\n", _class_devname);
|
||||
|
||||
|
|
|
|||
|
|
@ -51,7 +51,7 @@ class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBr
|
|||
public:
|
||||
static constexpr unsigned MAX_NAME_LEN = 20;
|
||||
|
||||
virtual ~IUavcanSensorBridge() { }
|
||||
virtual ~IUavcanSensorBridge() = default;
|
||||
|
||||
/**
|
||||
* Returns ASCII name of the bridge.
|
||||
|
|
@ -101,7 +101,7 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD
|
|||
bool _out_of_channels = false;
|
||||
|
||||
protected:
|
||||
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody
|
||||
static constexpr unsigned DEFAULT_MAX_CHANNELS = 5;
|
||||
|
||||
UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname,
|
||||
const orb_id_t orb_topic_sensor,
|
||||
|
|
|
|||
|
|
@ -55,7 +55,6 @@
|
|||
#include <arch/board/board.h>
|
||||
#include <arch/chip/chip.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
|
|
@ -77,23 +76,20 @@
|
|||
* UavcanNode
|
||||
*/
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev(UAVCAN_DEVICE_PATH),
|
||||
_node(can_driver, system_clock, _pool_allocator),
|
||||
_node_mutex(),
|
||||
_esc_controller(_node),
|
||||
_hardpoint_controller(_node),
|
||||
_time_sync_master(_node),
|
||||
_time_sync_slave(_node),
|
||||
_node_status_monitor(_node),
|
||||
_perf_control_latency(perf_alloc(PC_ELAPSED, "uavcan control latency")),
|
||||
_master_timer(_node),
|
||||
_setget_response(0)
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, "uavcan: cycle time")),
|
||||
_interval_perf(perf_alloc(PC_INTERVAL, "uavcan: cycle interval")),
|
||||
_control_latency_perf(perf_alloc(PC_ELAPSED, "uavcan: control latency")),
|
||||
_master_timer(_node)
|
||||
{
|
||||
_task_should_exit = false;
|
||||
_fw_server_action = None;
|
||||
_fw_server_status = -1;
|
||||
_tx_injector = nullptr;
|
||||
_control_topics[0] = ORB_ID(actuator_controls_0);
|
||||
_control_topics[1] = ORB_ID(actuator_controls_1);
|
||||
_control_topics[2] = ORB_ID(actuator_controls_2);
|
||||
|
|
@ -121,7 +117,6 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
|||
|
||||
UavcanNode::~UavcanNode()
|
||||
{
|
||||
|
||||
fw_server(Stop);
|
||||
|
||||
if (_task != -1) {
|
||||
|
|
@ -144,8 +139,6 @@ UavcanNode::~UavcanNode()
|
|||
} while (_task != -1);
|
||||
}
|
||||
|
||||
(void)orb_unsubscribe(_armed_sub);
|
||||
(void)orb_unsubscribe(_test_motor_sub);
|
||||
(void)orb_unsubscribe(_actuator_direct_sub);
|
||||
|
||||
// Removing the sensor bridges
|
||||
|
|
@ -161,10 +154,13 @@ UavcanNode::~UavcanNode()
|
|||
delete _mixers;
|
||||
}
|
||||
|
||||
perf_free(_perf_control_latency);
|
||||
perf_free(_cycle_perf);
|
||||
perf_free(_interval_perf);
|
||||
perf_free(_control_latency_perf);
|
||||
}
|
||||
|
||||
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
|
||||
int
|
||||
UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
|
||||
{
|
||||
int rv = -1;
|
||||
|
||||
|
|
@ -185,7 +181,8 @@ int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
|
|||
return rv;
|
||||
}
|
||||
|
||||
int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
|
||||
int
|
||||
UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
|
||||
{
|
||||
if (resp.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
|
||||
return std::printf("name: %s %lld\n", resp.name.c_str(),
|
||||
|
|
@ -207,7 +204,8 @@ int UavcanNode::print_params(uavcan::protocol::param::GetSet::Response &resp)
|
|||
return -1;
|
||||
}
|
||||
|
||||
void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
void
|
||||
UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
{
|
||||
uavcan::protocol::param::ExecuteOpcode::Response resp;
|
||||
_callback_success = result.isSuccessful();
|
||||
|
|
@ -215,7 +213,8 @@ void UavcanNode::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::par
|
|||
_callback_success &= resp.ok;
|
||||
}
|
||||
|
||||
int UavcanNode::save_params(int remote_node_id)
|
||||
int
|
||||
UavcanNode::save_params(int remote_node_id)
|
||||
{
|
||||
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
|
||||
opcode_req.opcode = opcode_req.OPCODE_SAVE;
|
||||
|
|
@ -238,7 +237,8 @@ int UavcanNode::save_params(int remote_node_id)
|
|||
return 0;
|
||||
}
|
||||
|
||||
void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
|
||||
void
|
||||
UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
|
||||
{
|
||||
uavcan::protocol::RestartNode::Response resp;
|
||||
_callback_success = result.isSuccessful();
|
||||
|
|
@ -246,7 +246,8 @@ void UavcanNode::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::Re
|
|||
_callback_success &= resp.ok;
|
||||
}
|
||||
|
||||
int UavcanNode::reset_node(int remote_node_id)
|
||||
int
|
||||
UavcanNode::reset_node(int remote_node_id)
|
||||
{
|
||||
uavcan::protocol::RestartNode::Request restart_req;
|
||||
restart_req.magic_number = restart_req.MAGIC_NUMBER;
|
||||
|
|
@ -269,7 +270,8 @@ int UavcanNode::reset_node(int remote_node_id)
|
|||
return 0;
|
||||
}
|
||||
|
||||
int UavcanNode::list_params(int remote_node_id)
|
||||
int
|
||||
UavcanNode::list_params(int remote_node_id)
|
||||
{
|
||||
int rv = 0;
|
||||
int index = 0;
|
||||
|
|
@ -299,14 +301,15 @@ int UavcanNode::list_params(int remote_node_id)
|
|||
return rv;
|
||||
}
|
||||
|
||||
|
||||
void UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
void
|
||||
UavcanNode::cb_setget(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
{
|
||||
_callback_success = result.isSuccessful();
|
||||
*_setget_response = result.getResponse();
|
||||
}
|
||||
|
||||
int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
|
||||
int
|
||||
UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::protocol::param::GetSet::Request &req)
|
||||
{
|
||||
if (name != nullptr) {
|
||||
req.name = name;
|
||||
|
|
@ -331,7 +334,8 @@ int UavcanNode::get_set_param(int remote_node_id, const char *name, uavcan::pro
|
|||
return call_res;
|
||||
}
|
||||
|
||||
int UavcanNode::set_param(int remote_node_id, const char *name, char *value)
|
||||
int
|
||||
UavcanNode::set_param(int remote_node_id, const char *name, char *value)
|
||||
{
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
uavcan::protocol::param::GetSet::Response resp;
|
||||
|
|
@ -398,7 +402,8 @@ int UavcanNode::set_param(int remote_node_id, const char *name, char *value)
|
|||
return rv;
|
||||
}
|
||||
|
||||
int UavcanNode::get_param(int remote_node_id, const char *name)
|
||||
int
|
||||
UavcanNode::get_param(int remote_node_id, const char *name)
|
||||
{
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
uavcan::protocol::param::GetSet::Response resp;
|
||||
|
|
@ -418,7 +423,8 @@ int UavcanNode::get_param(int remote_node_id, const char *name)
|
|||
return rv;
|
||||
}
|
||||
|
||||
void UavcanNode::update_params()
|
||||
void
|
||||
UavcanNode::update_params()
|
||||
{
|
||||
// multicopter air-mode
|
||||
param_t param_handle = param_find("MC_AIRMODE");
|
||||
|
|
@ -434,7 +440,8 @@ void UavcanNode::update_params()
|
|||
}
|
||||
}
|
||||
|
||||
int UavcanNode::start_fw_server()
|
||||
int
|
||||
UavcanNode::start_fw_server()
|
||||
{
|
||||
int rv = -1;
|
||||
_fw_server_action = Busy;
|
||||
|
|
@ -460,11 +467,12 @@ int UavcanNode::start_fw_server()
|
|||
return rv;
|
||||
}
|
||||
|
||||
int UavcanNode::request_fw_check()
|
||||
int
|
||||
UavcanNode::request_fw_check()
|
||||
{
|
||||
int rv = -1;
|
||||
_fw_server_action = Busy;
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
|
||||
if (_servers != nullptr) {
|
||||
_servers->requestCheckAllNodesFirmwareAndUpdate();
|
||||
|
|
@ -474,14 +482,14 @@ int UavcanNode::request_fw_check()
|
|||
_fw_server_action = None;
|
||||
px4_sem_post(&_server_command_sem);
|
||||
return rv;
|
||||
|
||||
}
|
||||
|
||||
int UavcanNode::stop_fw_server()
|
||||
int
|
||||
UavcanNode::stop_fw_server()
|
||||
{
|
||||
int rv = -1;
|
||||
_fw_server_action = Busy;
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
|
||||
if (_servers != nullptr) {
|
||||
/*
|
||||
|
|
@ -500,8 +508,8 @@ int UavcanNode::stop_fw_server()
|
|||
return rv;
|
||||
}
|
||||
|
||||
|
||||
int UavcanNode::fw_server(eServerAction action)
|
||||
int
|
||||
UavcanNode::fw_server(eServerAction action)
|
||||
{
|
||||
int rv = -EAGAIN;
|
||||
|
||||
|
|
@ -525,8 +533,8 @@ int UavcanNode::fw_server(eServerAction action)
|
|||
return rv;
|
||||
}
|
||||
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
int
|
||||
UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
{
|
||||
if (_instance != nullptr) {
|
||||
PX4_WARN("Already started");
|
||||
|
|
@ -580,8 +588,12 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t 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);
|
||||
_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) {
|
||||
PX4_ERR("start failed: %d", errno);
|
||||
|
|
@ -591,7 +603,8 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
|||
return OK;
|
||||
}
|
||||
|
||||
void UavcanNode::fill_node_info()
|
||||
void
|
||||
UavcanNode::fill_node_info()
|
||||
{
|
||||
/* software version */
|
||||
uavcan::protocol::SoftwareVersion swver;
|
||||
|
|
@ -614,13 +627,11 @@ void UavcanNode::fill_node_info()
|
|||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id)
|
||||
int
|
||||
UavcanNode::init(uavcan::NodeID node_id)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
// Do regular cdev init
|
||||
ret = CDev::init();
|
||||
int ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
|
|
@ -640,7 +651,7 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
|||
}
|
||||
|
||||
{
|
||||
(void) param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed);
|
||||
param_get(param_find("UAVCAN_ESC_IDLT"), &_idle_throttle_when_armed);
|
||||
_esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0);
|
||||
}
|
||||
|
||||
|
|
@ -668,7 +679,8 @@ int UavcanNode::init(uavcan::NodeID node_id)
|
|||
return _node.start();
|
||||
}
|
||||
|
||||
void UavcanNode::node_spin_once()
|
||||
void
|
||||
UavcanNode::node_spin_once()
|
||||
{
|
||||
const int spin_res = _node.spinOnce();
|
||||
|
||||
|
|
@ -686,7 +698,8 @@ void UavcanNode::node_spin_once()
|
|||
add a fd to the list of polled events. This assumes you want
|
||||
POLLIN for now.
|
||||
*/
|
||||
int UavcanNode::add_poll_fd(int fd)
|
||||
int
|
||||
UavcanNode::add_poll_fd(int fd)
|
||||
{
|
||||
int ret = _poll_fds_num;
|
||||
|
||||
|
|
@ -698,13 +711,13 @@ int UavcanNode::add_poll_fd(int fd)
|
|||
_poll_fds[_poll_fds_num].fd = fd;
|
||||
_poll_fds[_poll_fds_num].events = POLLIN;
|
||||
_poll_fds_num += 1;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
|
||||
void
|
||||
UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
|
||||
{
|
||||
|
||||
/*
|
||||
* Check whether there are higher priority masters in the network.
|
||||
* If there are, we need to activate the local slave in order to sync with them.
|
||||
|
|
@ -742,25 +755,19 @@ void UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
|
|||
_time_sync_master.publish();
|
||||
}
|
||||
|
||||
|
||||
|
||||
int UavcanNode::run()
|
||||
int
|
||||
UavcanNode::run()
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
// XXX figure out the output count
|
||||
_output_count = 2;
|
||||
|
||||
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
|
||||
_actuator_direct_sub = orb_subscribe(ORB_ID(actuator_direct));
|
||||
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
|
||||
/*
|
||||
* Set up the time synchronization
|
||||
*/
|
||||
actuator_outputs_s outputs{};
|
||||
|
||||
// Set up the time synchronization
|
||||
const int slave_init_res = _time_sync_slave.start();
|
||||
|
||||
if (slave_init_res < 0) {
|
||||
|
|
@ -811,15 +818,16 @@ int UavcanNode::run()
|
|||
|
||||
update_params();
|
||||
|
||||
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// check for parameter updates
|
||||
if (parameter_update_sub.updated()) {
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
parameter_update_sub.copy(&pupdate);
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
update_params();
|
||||
|
|
@ -878,30 +886,31 @@ int UavcanNode::run()
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
see if we have any direct actuator updates
|
||||
*/
|
||||
// see if we have any direct actuator updates
|
||||
actuator_direct_s actuator_direct{};
|
||||
|
||||
if (_actuator_direct_sub != -1 &&
|
||||
(_poll_fds[_actuator_direct_poll_fd_num].revents & POLLIN) &&
|
||||
orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &_actuator_direct) == OK &&
|
||||
orb_copy(ORB_ID(actuator_direct), _actuator_direct_sub, &actuator_direct) == OK &&
|
||||
!_test_in_progress) {
|
||||
if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
|
||||
_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
|
||||
|
||||
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));
|
||||
_outputs.noutputs = _actuator_direct.nvalues;
|
||||
memcpy(&outputs.output[0], &actuator_direct.values[0], actuator_direct.nvalues * sizeof(float));
|
||||
outputs.noutputs = actuator_direct.nvalues;
|
||||
new_output = true;
|
||||
}
|
||||
|
||||
// can we mix?
|
||||
if (_test_in_progress) {
|
||||
memset(&_outputs, 0, sizeof(_outputs));
|
||||
|
||||
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;
|
||||
|
|
@ -916,7 +925,7 @@ int UavcanNode::run()
|
|||
_mixers->set_airmode(_airmode);
|
||||
|
||||
// Do mixing
|
||||
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
|
||||
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
|
||||
|
||||
new_output = true;
|
||||
}
|
||||
|
|
@ -924,31 +933,25 @@ int UavcanNode::run()
|
|||
|
||||
if (new_output) {
|
||||
// iterate actuators, checking for valid values
|
||||
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
|
||||
for (uint8_t i = 0; i < outputs.noutputs; i++) {
|
||||
|
||||
// last resort: catch NaN, INF and out-of-band errors
|
||||
if (!isfinite(_outputs.output[i])) {
|
||||
if (!isfinite(outputs.output[i])) {
|
||||
/*
|
||||
* Value is NaN, INF or out of band - set to the minimum value.
|
||||
* This will be clearly visible on the servo status and will limit the risk of accidentally
|
||||
* spinning motors. It would be deadly in flight.
|
||||
*/
|
||||
_outputs.output[i] = -1.0f;
|
||||
outputs.output[i] = -1.0f;
|
||||
}
|
||||
|
||||
// never go below min
|
||||
if (_outputs.output[i] < -1.0f) {
|
||||
_outputs.output[i] = -1.0f;
|
||||
}
|
||||
|
||||
// never go above max
|
||||
if (_outputs.output[i] > 1.0f) {
|
||||
_outputs.output[i] = 1.0f;
|
||||
}
|
||||
// never go below min or max
|
||||
outputs.output[i] = math::constrain(outputs.output[i], -1.0f, 1.0f);
|
||||
}
|
||||
|
||||
// Output to the bus
|
||||
_esc_controller.update_outputs(_outputs.output, _outputs.noutputs);
|
||||
_outputs.timestamp = hrt_absolute_time();
|
||||
_esc_controller.update_outputs(outputs.output, outputs.noutputs);
|
||||
outputs.timestamp = hrt_absolute_time();
|
||||
|
||||
// use first valid timestamp_sample for latency tracking
|
||||
for (int i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
|
||||
|
|
@ -956,19 +959,15 @@ int UavcanNode::run()
|
|||
const hrt_abstime ×tamp_sample = _controls[i].timestamp_sample;
|
||||
|
||||
if (required && (timestamp_sample > 0)) {
|
||||
perf_set_elapsed(_perf_control_latency, _outputs.timestamp - timestamp_sample);
|
||||
perf_set_elapsed(_control_latency_perf, outputs.timestamp - timestamp_sample);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Check motor test state
|
||||
bool updated = false;
|
||||
orb_check(_test_motor_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
|
||||
if (_test_motor_sub.updated()) {
|
||||
_test_motor_sub.copy(&_test_motor);
|
||||
|
||||
// Update the test status and check that we're not locked down
|
||||
_test_in_progress = (_test_motor.action == test_motor_s::ACTION_RUN);
|
||||
|
|
@ -976,24 +975,25 @@ int UavcanNode::run()
|
|||
}
|
||||
|
||||
// Check arming state
|
||||
orb_check(_armed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
|
||||
if (_armed_sub.updated()) {
|
||||
actuator_armed_s armed{};
|
||||
_armed_sub.copy(&armed);
|
||||
|
||||
// Update the armed status and check that we're not locked down and motor
|
||||
// test is not running
|
||||
bool set_armed = _armed.armed && !_armed.lockdown && !_armed.manual_lockdown && !_test_in_progress;
|
||||
bool set_armed = (armed.armed && !armed.lockdown && !armed.manual_lockdown && !_test_in_progress);
|
||||
|
||||
arm_actuators(set_armed);
|
||||
|
||||
if (_armed.soft_stop) {
|
||||
if (armed.soft_stop) {
|
||||
_esc_controller.enable_idle_throttle_when_armed(false);
|
||||
|
||||
} else {
|
||||
_esc_controller.enable_idle_throttle_when_armed(_idle_throttle_when_armed > 0);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
}
|
||||
|
||||
(void)::close(busevent_fd);
|
||||
|
|
@ -1024,7 +1024,7 @@ UavcanNode::teardown()
|
|||
}
|
||||
}
|
||||
|
||||
return (_armed_sub >= 0) ? orb_unsubscribe(_armed_sub) : 0;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -1032,6 +1032,7 @@ UavcanNode::arm_actuators(bool arm)
|
|||
{
|
||||
_is_armed = arm;
|
||||
_esc_controller.arm_all_escs(arm);
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
@ -1186,6 +1187,8 @@ UavcanNode::print_info()
|
|||
printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
|
||||
printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
|
||||
|
||||
printf("\n");
|
||||
|
||||
// UAVCAN node perfcounters
|
||||
printf("UAVCAN node status:\n");
|
||||
printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
|
||||
|
|
@ -1193,6 +1196,8 @@ UavcanNode::print_info()
|
|||
printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
|
||||
printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
|
||||
|
||||
printf("\n");
|
||||
|
||||
// CAN driver status
|
||||
for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
|
||||
printf("CAN%u status:\n", unsigned(i + 1));
|
||||
|
|
@ -1206,28 +1211,14 @@ UavcanNode::print_info()
|
|||
printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
// ESC mixer status
|
||||
printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n",
|
||||
printf("ESC actuators control groups: sub: %X / req: %X / fds: %u\n",
|
||||
(unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
|
||||
printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK");
|
||||
|
||||
if (_outputs.noutputs != 0) {
|
||||
PX4_INFO("ESC output: ");
|
||||
|
||||
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
|
||||
printf("%d ", (int)(_outputs.output[i] * 1000));
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
// ESC status
|
||||
int esc_sub = orb_subscribe(ORB_ID(esc_status));
|
||||
esc_status_s esc = {};
|
||||
orb_copy(ORB_ID(esc_status), esc_sub, &esc);
|
||||
orb_unsubscribe(esc_sub);
|
||||
|
||||
print_message(esc);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
// Sensor bridges
|
||||
for (const auto &br : _sensor_bridges) {
|
||||
|
|
@ -1237,33 +1228,38 @@ UavcanNode::print_info()
|
|||
}
|
||||
|
||||
// Printing all nodes that are online
|
||||
std::printf("Online nodes (Node ID, Health, Mode):\n");
|
||||
printf("Online nodes (Node ID, Health, Mode):\n");
|
||||
_node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) {
|
||||
static constexpr const char *HEALTH[] = {
|
||||
"OK", "WARN", "ERR", "CRIT"
|
||||
};
|
||||
static constexpr const char *MODES[] = {
|
||||
"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"
|
||||
};
|
||||
std::printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]);
|
||||
static constexpr const char *HEALTH[] = {"OK", "WARN", "ERR", "CRIT"};
|
||||
static constexpr const char *MODES[] = {"OPERAT", "INIT", "MAINT", "SW_UPD", "?", "?", "?", "OFFLN"};
|
||||
printf("\t% 3d %-10s %-10s\n", int(nid.get()), HEALTH[ns.health], MODES[ns.mode]);
|
||||
});
|
||||
|
||||
printf("\n");
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
perf_print_counter(_interval_perf);
|
||||
perf_print_counter(_control_latency_perf);
|
||||
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void UavcanNode::shrink()
|
||||
void
|
||||
UavcanNode::shrink()
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
_pool_allocator.shrink();
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
|
||||
void
|
||||
UavcanNode::hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command)
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
_hardpoint_controller.set_command(hardpoint_id, command);
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
/*
|
||||
* App entry point
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -45,6 +45,12 @@
|
|||
#include <px4_config.h>
|
||||
|
||||
#include "uavcan_driver.hpp"
|
||||
#include "uavcan_servers.hpp"
|
||||
#include "allocator.hpp"
|
||||
#include "actuators/esc.hpp"
|
||||
#include "actuators/hardpoint.hpp"
|
||||
#include "sensors/sensor_bridge.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>
|
||||
|
|
@ -53,21 +59,16 @@
|
|||
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
|
||||
#include <uavcan/protocol/RestartNode.hpp>
|
||||
|
||||
#include <drivers/device/device.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <lib/drivers/device/device.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
#include <uORB/topics/actuator_controls.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"
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/test_motor.h>
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4
|
||||
|
||||
|
|
@ -131,10 +132,10 @@ public:
|
|||
|
||||
void hardpoint_controller_set(uint8_t hardpoint_id, uint16_t command);
|
||||
|
||||
static UavcanNode *instance() { return _instance; }
|
||||
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 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);
|
||||
|
|
@ -146,42 +147,38 @@ private:
|
|||
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
|
||||
void set_setget_response(uavcan::protocol::param::GetSet::Response *resp) { _setget_response = resp; }
|
||||
void free_setget_response(void) { _setget_response = nullptr; }
|
||||
|
||||
int _test_motor_sub = -1; ///< uORB subscription of the test_motor status
|
||||
test_motor_s _test_motor = {};
|
||||
bool _test_in_progress = false;
|
||||
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{None};
|
||||
int _fw_server_status{-1};
|
||||
|
||||
unsigned _output_count = 0; ///< number of actuators currently available
|
||||
bool _is_armed{false}; ///< the arming status of the actuators on the bus
|
||||
|
||||
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;
|
||||
pthread_mutex_t _node_mutex{};
|
||||
px4_sem_t _server_command_sem;
|
||||
UavcanEscController _esc_controller;
|
||||
UavcanHardpointController _hardpoint_controller;
|
||||
|
|
@ -191,44 +188,52 @@ private:
|
|||
|
||||
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;
|
||||
MixerGroup *_mixers{nullptr};
|
||||
ITxQueueInjector *_tx_injector{nullptr};
|
||||
|
||||
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 = {};
|
||||
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};
|
||||
|
||||
actuator_outputs_s _outputs = {};
|
||||
uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _test_motor_sub{ORB_ID(test_motor)};
|
||||
|
||||
perf_counter_t _perf_control_latency;
|
||||
int _actuator_direct_sub{-1}; ///< uORB subscription of the actuator_direct topic
|
||||
uint8_t _actuator_direct_poll_fd_num{0};
|
||||
|
||||
Mixer::Airmode _airmode = Mixer::Airmode::disabled;
|
||||
float _thr_mdl_factor = 0.0f;
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
perf_counter_t _control_latency_perf;
|
||||
|
||||
Mixer::Airmode _airmode{Mixer::Airmode::disabled};
|
||||
float _thr_mdl_factor{0.0f};
|
||||
|
||||
// index into _poll_fds for each _control_subs handle
|
||||
uint8_t _poll_ids[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN];
|
||||
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;
|
||||
bool _callback_success{false};
|
||||
|
||||
uavcan::protocol::param::GetSet::Response *_setget_response{nullptr};
|
||||
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -60,11 +60,9 @@
|
|||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
#include <uORB/topics/uavcan_parameter_request.h>
|
||||
#include <uORB/topics/uavcan_parameter_value.h>
|
||||
|
||||
#include <v2.0/common/mavlink.h>
|
||||
|
||||
|
||||
/**
|
||||
* @file uavcan_servers.cpp
|
||||
*
|
||||
|
|
@ -78,8 +76,8 @@
|
|||
* UavcanNode
|
||||
*/
|
||||
UavcanServers *UavcanServers::_instance;
|
||||
|
||||
UavcanServers::UavcanServers(uavcan::INode &main_node) :
|
||||
_subnode_thread(-1),
|
||||
_vdriver(NumIfaces, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator(), VirtualIfaceBlockAllocationQuota),
|
||||
_subnode(_vdriver, UAVCAN_DRIVER::SystemClock::instance(), main_node.getAllocator()),
|
||||
_main_node(main_node),
|
||||
|
|
@ -96,7 +94,6 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) :
|
|||
_enumeration_client(_subnode),
|
||||
_enumeration_getset_client(_subnode),
|
||||
_enumeration_save_client(_subnode)
|
||||
|
||||
{
|
||||
}
|
||||
|
||||
|
|
@ -109,17 +106,18 @@ UavcanServers::~UavcanServers()
|
|||
_main_node.getDispatcher().removeRxFrameListener();
|
||||
}
|
||||
|
||||
int UavcanServers::stop()
|
||||
int
|
||||
UavcanServers::stop()
|
||||
{
|
||||
UavcanServers *server = instance();
|
||||
|
||||
if (server == nullptr) {
|
||||
warnx("Already stopped");
|
||||
PX4_INFO("Already stopped");
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (server->_subnode_thread) {
|
||||
warnx("stopping fw srv thread...");
|
||||
PX4_INFO("stopping fw srv thread...");
|
||||
server->_subnode_thread_should_exit = true;
|
||||
(void)pthread_join(server->_subnode_thread, NULL);
|
||||
}
|
||||
|
|
@ -132,10 +130,11 @@ int UavcanServers::stop()
|
|||
return 0;
|
||||
}
|
||||
|
||||
int UavcanServers::start(uavcan::INode &main_node)
|
||||
int
|
||||
UavcanServers::start(uavcan::INode &main_node)
|
||||
{
|
||||
if (_instance != nullptr) {
|
||||
warnx("Already started");
|
||||
PX4_INFO("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
|
@ -145,14 +144,14 @@ int UavcanServers::start(uavcan::INode &main_node)
|
|||
_instance = new UavcanServers(main_node);
|
||||
|
||||
if (_instance == nullptr) {
|
||||
warnx("Out of memory");
|
||||
PX4_ERR("Out of memory");
|
||||
return -2;
|
||||
}
|
||||
|
||||
int rv = _instance->init();
|
||||
int rv = _instance->init();
|
||||
|
||||
if (rv < 0) {
|
||||
warnx("Node init failed: %d", rv);
|
||||
PX4_ERR("Node init failed: %d", rv);
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
return rv;
|
||||
|
|
@ -170,7 +169,7 @@ int UavcanServers::start(uavcan::INode &main_node)
|
|||
param.sched_priority = Priority;
|
||||
|
||||
if (pthread_attr_setschedparam(&tattr, ¶m)) {
|
||||
warnx("setting sched params failed");
|
||||
PX4_ERR("setting sched params failed");
|
||||
}
|
||||
|
||||
static auto run_trampoline = [](void *) {return UavcanServers::_instance->run(_instance);};
|
||||
|
|
@ -179,7 +178,7 @@ int UavcanServers::start(uavcan::INode &main_node)
|
|||
|
||||
if (rv != 0) {
|
||||
rv = -rv;
|
||||
warnx("pthread_create() failed: %d", rv);
|
||||
PX4_ERR("pthread_create() failed: %d", rv);
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
}
|
||||
|
|
@ -187,7 +186,8 @@ int UavcanServers::start(uavcan::INode &main_node)
|
|||
return rv;
|
||||
}
|
||||
|
||||
int UavcanServers::init()
|
||||
int
|
||||
UavcanServers::init()
|
||||
{
|
||||
errno = 0;
|
||||
|
||||
|
|
@ -195,11 +195,10 @@ int UavcanServers::init()
|
|||
* Initialize the mutex.
|
||||
* giving it its path
|
||||
*/
|
||||
|
||||
int ret = Lock::init(_subnode_mutex);
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("Lock init: %d", errno);
|
||||
PX4_ERR("Lock init: %d", errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -208,7 +207,6 @@ int UavcanServers::init()
|
|||
_subnode.setNodeID(_main_node.getNodeID());
|
||||
_main_node.getDispatcher().installRxFrameListener(&_vdriver);
|
||||
|
||||
|
||||
/*
|
||||
* Initialize the fw version checker.
|
||||
* giving it its path
|
||||
|
|
@ -216,7 +214,7 @@ int UavcanServers::init()
|
|||
ret = _fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
|
||||
PX4_ERR("FirmwareVersionChecker init: %d, errno: %d", ret, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -225,7 +223,7 @@ int UavcanServers::init()
|
|||
ret = _fw_server.start();
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("BasicFileServer init: %d, errno: %d", ret, errno);
|
||||
PX4_ERR("BasicFileServer init: %d, errno: %d", ret, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -234,7 +232,7 @@ int UavcanServers::init()
|
|||
ret = _storage_backend.init(UAVCAN_NODE_DB_PATH);
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("FileStorageBackend init: %d, errno: %d", ret, errno);
|
||||
PX4_ERR("FileStorageBackend init: %d, errno: %d", ret, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -243,7 +241,7 @@ int UavcanServers::init()
|
|||
ret = _tracer.init(UAVCAN_LOG_FILE);
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("FileEventTracer init: %d, errno: %d", ret, errno);
|
||||
PX4_ERR("FileEventTracer init: %d, errno: %d", ret, errno);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -255,34 +253,32 @@ int UavcanServers::init()
|
|||
ret = _server_instance.init(hwver.unique_id);
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("CentralizedServer init: %d", ret);
|
||||
PX4_ERR("CentralizedServer init: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start node info retriever to fetch node info from new nodes */
|
||||
|
||||
ret = _node_info_retriever.start();
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("NodeInfoRetriever init: %d", ret);
|
||||
PX4_ERR("NodeInfoRetriever init: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start the fw version checker */
|
||||
|
||||
ret = _fw_upgrade_trigger.start(_node_info_retriever, _fw_version_checker.getFirmwarePath());
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("FirmwareUpdateTrigger init: %d", ret);
|
||||
PX4_ERR("FirmwareUpdateTrigger init: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Start the Node */
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
pthread_addr_t
|
||||
UavcanServers::run(pthread_addr_t)
|
||||
{
|
||||
prctl(PR_SET_NAME, "uavcan fw srv", 0);
|
||||
|
||||
|
|
@ -296,9 +292,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
|
||||
/* the subscribe call needs to happen in the same thread,
|
||||
* so not in the constructor */
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request));
|
||||
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
|
||||
uORB::Subscription armed_sub{ORB_ID(actuator_armed)};
|
||||
uORB::Subscription vcmd_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription param_request_sub{ORB_ID(uavcan_parameter_request)};
|
||||
|
||||
/* Set up shared service clients */
|
||||
_param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset));
|
||||
|
|
@ -326,16 +322,13 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10));
|
||||
|
||||
if (spin_res < 0) {
|
||||
warnx("node spin error %i", spin_res);
|
||||
PX4_ERR("node spin error %i", spin_res);
|
||||
}
|
||||
|
||||
// Check for parameter requests (get/set/list)
|
||||
bool param_request_ready;
|
||||
orb_check(param_request_sub, ¶m_request_ready);
|
||||
|
||||
if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
|
||||
struct uavcan_parameter_request_s request;
|
||||
orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request);
|
||||
if (param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
|
||||
uavcan_parameter_request_s request{};
|
||||
param_request_sub.copy(&request);
|
||||
|
||||
if (_param_counts[request.node_id]) {
|
||||
/*
|
||||
|
|
@ -355,7 +348,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
int call_res = _param_getset_client.call(request.node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
|
|
@ -388,7 +381,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
int call_res = _param_getset_client.call(request.node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
|
|
@ -402,7 +395,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
_param_list_node_id = request.node_id;
|
||||
_param_list_all_nodes = false;
|
||||
|
||||
warnx("UAVCAN command bridge: starting component-specific param list");
|
||||
PX4_INFO("UAVCAN command bridge: starting component-specific param list");
|
||||
}
|
||||
|
||||
} else if (request.node_id == MAV_COMP_ID_ALL) {
|
||||
|
|
@ -416,7 +409,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
_param_list_node_id = get_next_active_node_id(0);
|
||||
_param_list_all_nodes = true;
|
||||
|
||||
warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
|
||||
PX4_INFO("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id);
|
||||
|
||||
if (_param_counts[_param_list_node_id] == 0) {
|
||||
param_count(_param_list_node_id);
|
||||
|
|
@ -435,7 +428,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
// Handle parameter listing index/node ID advancement
|
||||
if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
|
||||
if (_param_index >= _param_counts[_param_list_node_id]) {
|
||||
warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
|
||||
PX4_INFO("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id);
|
||||
// Reached the end of the current node's parameter set.
|
||||
_param_list_in_progress = false;
|
||||
|
||||
|
|
@ -457,7 +450,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
// Keep on listing.
|
||||
_param_index = 0;
|
||||
_param_list_in_progress = true;
|
||||
warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
|
||||
PX4_INFO("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -474,7 +467,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
|
||||
if (call_res < 0) {
|
||||
_param_list_in_progress = false;
|
||||
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
|
|
@ -482,12 +475,9 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
}
|
||||
|
||||
// Check for ESC enumeration commands
|
||||
bool cmd_ready;
|
||||
orb_check(cmd_sub, &cmd_ready);
|
||||
|
||||
if (cmd_ready && !_cmd_in_progress) {
|
||||
struct vehicle_command_s cmd;
|
||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
if (vcmd_sub.updated() && !_cmd_in_progress) {
|
||||
vehicle_command_s cmd{};
|
||||
vcmd_sub.copy(&cmd);
|
||||
|
||||
uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
|
||||
|
||||
|
|
@ -496,7 +486,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
int node_id = static_cast<int>(cmd.param2 + 0.5f);
|
||||
int call_res;
|
||||
|
||||
warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
|
||||
PX4_INFO("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id);
|
||||
|
||||
switch (command_id) {
|
||||
case 0:
|
||||
|
|
@ -512,7 +502,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
call_res = _enumeration_client.call(get_next_active_node_id(0), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res);
|
||||
beep(BeepFrequencyError);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
|
||||
|
||||
|
|
@ -524,7 +514,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
}
|
||||
|
||||
default: {
|
||||
warnx("UAVCAN command bridge: unknown command ID %d", command_id);
|
||||
PX4_ERR("UAVCAN command bridge: unknown command ID %d", command_id);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
|
||||
break;
|
||||
}
|
||||
|
|
@ -533,7 +523,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
|
||||
int command_id = static_cast<int>(cmd.param1 + 0.5f);
|
||||
|
||||
warnx("UAVCAN command bridge: received storage command ID %d", command_id);
|
||||
PX4_INFO("UAVCAN command bridge: received storage command ID %d", command_id);
|
||||
|
||||
switch (command_id) {
|
||||
case 1: {
|
||||
|
|
@ -576,15 +566,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
|
||||
// Shut down once armed
|
||||
// TODO (elsewhere): start up again once disarmed?
|
||||
bool updated;
|
||||
orb_check(armed_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
struct actuator_armed_s armed;
|
||||
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
|
||||
if (armed_sub.updated()) {
|
||||
actuator_armed_s armed{};
|
||||
armed_sub.copy(&armed);
|
||||
|
||||
if (armed.armed && !(armed.lockdown || armed.manual_lockdown)) {
|
||||
warnx("UAVCAN command bridge: system armed, exiting now.");
|
||||
PX4_INFO("UAVCAN command bridge: system armed, exiting now.");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -592,11 +579,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
|||
|
||||
_subnode_thread_should_exit = false;
|
||||
|
||||
warnx("exiting");
|
||||
PX4_INFO("exiting");
|
||||
return (pthread_addr_t) 0;
|
||||
}
|
||||
|
||||
void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
void
|
||||
UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
{
|
||||
if (_count_in_progress) {
|
||||
/*
|
||||
|
|
@ -622,14 +610,14 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
if (call_res < 0) {
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res);
|
||||
beep(BeepFrequencyError);
|
||||
}
|
||||
|
||||
} else {
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
|
||||
PX4_INFO("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]);
|
||||
beep(BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
|
|
@ -637,7 +625,7 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
_param_counts[node_id] = 0;
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
warnx("UAVCAN command bridge: GetSet error during param count");
|
||||
PX4_ERR("UAVCAN command bridge: GetSet error during param count");
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -669,15 +657,10 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
|
||||
}
|
||||
|
||||
if (_param_response_pub == nullptr) {
|
||||
_param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response);
|
||||
}
|
||||
_param_response_pub.publish(response);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: GetSet error");
|
||||
PX4_ERR("UAVCAN command bridge: GetSet error");
|
||||
}
|
||||
|
||||
_param_in_progress = false;
|
||||
|
|
@ -685,38 +668,41 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanServers::param_count(uavcan::NodeID node_id)
|
||||
void
|
||||
UavcanServers::param_count(uavcan::NodeID node_id)
|
||||
{
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.index = 0;
|
||||
int call_res = _param_getset_client.call(node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't start parameter count: %d", call_res);
|
||||
|
||||
} else {
|
||||
_count_in_progress = true;
|
||||
_count_index = 0;
|
||||
warnx("UAVCAN command bridge: starting param count");
|
||||
PX4_INFO("UAVCAN command bridge: starting param count");
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::param_opcode(uavcan::NodeID node_id)
|
||||
void
|
||||
UavcanServers::param_opcode(uavcan::NodeID node_id)
|
||||
{
|
||||
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
|
||||
opcode_req.opcode = _param_save_opcode;
|
||||
int call_res = _param_opcode_client.call(node_id, opcode_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res);
|
||||
|
||||
} else {
|
||||
_cmd_in_progress = true;
|
||||
warnx("UAVCAN command bridge: sent ExecuteOpcode");
|
||||
PX4_INFO("UAVCAN command bridge: sent ExecuteOpcode");
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
void
|
||||
UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
{
|
||||
bool success = result.isSuccessful();
|
||||
uint8_t node_id = result.getCallID().server_node_id.get();
|
||||
|
|
@ -725,23 +711,23 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
_cmd_in_progress = false;
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
|
||||
PX4_ERR("UAVCAN command bridge: save request for node %hhu timed out.", node_id);
|
||||
|
||||
} else if (!result.getResponse().ok) {
|
||||
warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
|
||||
PX4_ERR("UAVCAN command bridge: save request for node %hhu rejected.", node_id);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
|
||||
PX4_INFO("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id);
|
||||
|
||||
uavcan::protocol::RestartNode::Request restart_req;
|
||||
restart_req.magic_number = restart_req.MAGIC_NUMBER;
|
||||
int call_res = _param_restartnode_client.call(node_id, restart_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
|
||||
PX4_ERR("UAVCAN command bridge: couldn't send RestartNode: %d", call_res);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: sent RestartNode");
|
||||
PX4_ERR("UAVCAN command bridge: sent RestartNode");
|
||||
_cmd_in_progress = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -761,7 +747,8 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
|
|||
}
|
||||
}
|
||||
|
||||
void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
|
||||
void
|
||||
UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &result)
|
||||
{
|
||||
bool success = result.isSuccessful();
|
||||
uint8_t node_id = result.getCallID().server_node_id.get();
|
||||
|
|
@ -770,12 +757,13 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
|
|||
_cmd_in_progress = false;
|
||||
|
||||
if (success) {
|
||||
warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
|
||||
PX4_INFO("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id);
|
||||
|
||||
// Clear the dirty flag
|
||||
clear_node_params_dirty(node_id);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
|
||||
PX4_ERR("UAVCAN command bridge: restart request for node %hhu failed.", node_id);
|
||||
}
|
||||
|
||||
// Get the next dirty node ID and send the same command to it
|
||||
|
|
@ -786,7 +774,8 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
|
|||
}
|
||||
}
|
||||
|
||||
uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
|
||||
uint8_t
|
||||
UavcanServers::get_next_active_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
|
|
@ -796,7 +785,8 @@ uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
|
|||
return base;
|
||||
}
|
||||
|
||||
uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
|
||||
uint8_t
|
||||
UavcanServers::get_next_dirty_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
|
|
@ -805,7 +795,8 @@ uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
|
|||
return base;
|
||||
}
|
||||
|
||||
void UavcanServers::beep(float frequency)
|
||||
void
|
||||
UavcanServers::beep(float frequency)
|
||||
{
|
||||
uavcan::equipment::indication::BeepCommand cmd;
|
||||
cmd.frequency = frequency;
|
||||
|
|
@ -813,20 +804,21 @@ void UavcanServers::beep(float frequency)
|
|||
(void)_beep_pub.broadcast(cmd);
|
||||
}
|
||||
|
||||
void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
|
||||
void
|
||||
UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &result)
|
||||
{
|
||||
uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get());
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
|
||||
} else if (result.getResponse().error) {
|
||||
warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
|
||||
result.getResponse().error);
|
||||
PX4_ERR("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(),
|
||||
result.getResponse().error);
|
||||
|
||||
} else {
|
||||
_esc_count++;
|
||||
warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
PX4_INFO("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
}
|
||||
|
||||
if (next_id < 128) {
|
||||
|
|
@ -840,22 +832,23 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan:
|
|||
int call_res = _enumeration_client.call(next_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent Begin request");
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent Begin request");
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: begun enumeration on all nodes.");
|
||||
PX4_INFO("UAVCAN ESC enumeration: begun enumeration on all nodes.");
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::cb_enumeration_indication(const
|
||||
uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication> &msg)
|
||||
void
|
||||
UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>
|
||||
&msg)
|
||||
{
|
||||
// Called whenever an ESC thinks it has received user input.
|
||||
warnx("UAVCAN ESC enumeration: got indication");
|
||||
PX4_INFO("UAVCAN ESC enumeration: got indication");
|
||||
|
||||
if (!_esc_enumeration_active) {
|
||||
// Ignore any messages received when we're not expecting them
|
||||
|
|
@ -867,33 +860,33 @@ void UavcanServers::cb_enumeration_indication(const
|
|||
|
||||
for (; i < _esc_enumeration_index; i++) {
|
||||
if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) {
|
||||
warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
|
||||
PX4_INFO("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d, ignored", _esc_enumeration_ids[i], i);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
req.name =
|
||||
msg.parameter_name; // 'esc_index' or something alike, the name is not standardized
|
||||
req.name = msg.parameter_name; // 'esc_index' or something alike, the name is not standardized
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = i;
|
||||
|
||||
int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
void
|
||||
UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result)
|
||||
{
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
|
||||
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
|
||||
uint8_t esc_index = (uint8_t)resp.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
|
||||
|
|
@ -907,41 +900,43 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan
|
|||
int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
|
||||
esc_index);
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
|
||||
esc_index);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
void
|
||||
UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result)
|
||||
{
|
||||
const bool this_is_the_last_one =
|
||||
_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 ||
|
||||
_esc_enumeration_index >= _esc_count;
|
||||
(_esc_enumeration_index >= uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1) ||
|
||||
(_esc_enumeration_index >= _esc_count);
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get());
|
||||
beep(BeepFrequencyError);
|
||||
|
||||
} else if (!result.getResponse().ok) {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
|
||||
PX4_ERR("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get());
|
||||
beep(BeepFrequencyError);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
PX4_INFO("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
|
||||
PX4_INFO("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count);
|
||||
|
||||
if (this_is_the_last_one) {
|
||||
_esc_enumeration_active = false;
|
||||
|
||||
// Tell all ESCs to stop enumerating
|
||||
uavcan::protocol::enumeration::Begin::Request req;
|
||||
|
||||
// TODO: Incorrect implementation; the parameter name field should be left empty.
|
||||
// Leaving it as-is to avoid breaking compatibility with non-compliant nodes.
|
||||
req.parameter_name = "esc_index";
|
||||
|
|
@ -949,15 +944,16 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::
|
|||
int call_res = _enumeration_client.call(get_next_active_node_id(0), req);
|
||||
|
||||
if (call_res < 0) {
|
||||
warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
|
||||
PX4_ERR("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
|
||||
PX4_INFO("UAVCAN ESC enumeration: sent Begin request to stop enumeration");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
|
||||
void
|
||||
UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_path)
|
||||
{
|
||||
/*
|
||||
Copy the ROMFS firmware directory to the appropriate location on SD, without
|
||||
|
|
@ -1025,14 +1021,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len;
|
||||
|
||||
if (srcpath_dev_len > maxlen) {
|
||||
warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
|
||||
PX4_WARN("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name);
|
||||
continue;
|
||||
}
|
||||
|
||||
size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len;
|
||||
|
||||
if (dstpath_dev_len > maxlen) {
|
||||
warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
|
||||
PX4_WARN("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1044,7 +1040,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("dev: couldn't create '%s'", dstpath);
|
||||
PX4_ERR("dev: couldn't create '%s'", dstpath);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
|
@ -1056,7 +1052,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
DIR *const dev_dir = opendir(srcpath);
|
||||
|
||||
if (!dev_dir) {
|
||||
warnx("dev: couldn't open '%s'", srcpath);
|
||||
PX4_ERR("dev: couldn't open '%s'", srcpath);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1074,14 +1070,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len;
|
||||
|
||||
if (srcpath_ver_len > maxlen) {
|
||||
warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
|
||||
PX4_ERR("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name);
|
||||
continue;
|
||||
}
|
||||
|
||||
size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len;
|
||||
|
||||
if (dstpath_ver_len > maxlen) {
|
||||
warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
|
||||
PX4_ERR("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1093,7 +1089,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("ver: couldn't create '%s'", dstpath);
|
||||
PX4_ERR("ver: couldn't create '%s'", dstpath);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
|
@ -1107,7 +1103,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
DIR *const src_ver_dir = opendir(srcpath);
|
||||
|
||||
if (!src_ver_dir) {
|
||||
warnx("ver: couldn't open '%s'", srcpath);
|
||||
PX4_ERR("ver: couldn't open '%s'", srcpath);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
|
@ -1129,7 +1125,7 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
DIR *const dst_ver_dir = opendir(dstpath);
|
||||
|
||||
if (!dst_ver_dir) {
|
||||
warnx("unlink: couldn't open '%s'", dstpath);
|
||||
PX4_ERR("unlink: couldn't open '%s'", dstpath);
|
||||
|
||||
} else {
|
||||
struct dirent *fw_dirent = NULL;
|
||||
|
|
@ -1153,14 +1149,14 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
|
||||
if (dstpath_fw_len > maxlen) {
|
||||
// sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator
|
||||
warnx("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
|
||||
PX4_ERR("unlink: path '%s/%s' too long", dstpath, fw_dirent->d_name);
|
||||
|
||||
} else {
|
||||
// File name starts with "_", delete it.
|
||||
dstpath[dstpath_ver_len] = '/';
|
||||
memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, dst_fw_len + 1);
|
||||
unlink(dstpath);
|
||||
warnx("unlink: removed '%s'", dstpath);
|
||||
PX4_ERR("unlink: removed '%s'", dstpath);
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -1178,10 +1174,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
size_t dstpath_fw_len = dstpath_ver_len + fw_len;
|
||||
|
||||
if (srcpath_fw_len > maxlen) {
|
||||
warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
|
||||
PX4_ERR("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name);
|
||||
|
||||
} else if (dstpath_fw_len > maxlen) {
|
||||
warnx("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
|
||||
PX4_ERR("copy: dstpath '%s/%s' too long", dstpath, src_fw_dirent->d_name);
|
||||
|
||||
} else {
|
||||
// All OK, make the paths and copy the file
|
||||
|
|
@ -1194,10 +1190,10 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
rv = copyFw(dstpath, srcpath);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
|
||||
PX4_ERR("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
|
||||
|
||||
} else {
|
||||
warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
|
||||
PX4_INFO("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1211,24 +1207,24 @@ void UavcanServers::unpackFwFromROMFS(const char *sd_path, const char *romfs_pat
|
|||
(void)closedir(romfs_dir);
|
||||
}
|
||||
|
||||
int UavcanServers::copyFw(const char *dst, const char *src)
|
||||
int
|
||||
UavcanServers::copyFw(const char *dst, const char *src)
|
||||
{
|
||||
int rv = 0;
|
||||
int dfd, sfd;
|
||||
uint8_t buffer[512];
|
||||
uint8_t buffer[512] {};
|
||||
|
||||
dfd = open(dst, O_WRONLY | O_CREAT, 0666);
|
||||
int dfd = open(dst, O_WRONLY | O_CREAT, 0666);
|
||||
|
||||
if (dfd < 0) {
|
||||
warnx("copyFw: couldn't open dst");
|
||||
PX4_ERR("copyFw: couldn't open dst");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
sfd = open(src, O_RDONLY, 0);
|
||||
int sfd = open(src, O_RDONLY, 0);
|
||||
|
||||
if (sfd < 0) {
|
||||
(void)close(dfd);
|
||||
warnx("copyFw: couldn't open src");
|
||||
PX4_ERR("copyFw: couldn't open src");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
|
|
@ -1238,7 +1234,7 @@ int UavcanServers::copyFw(const char *dst, const char *src)
|
|||
size = read(sfd, buffer, sizeof(buffer));
|
||||
|
||||
if (size < 0) {
|
||||
warnx("copyFw: couldn't read");
|
||||
PX4_ERR("copyFw: couldn't read");
|
||||
rv = -errno;
|
||||
|
||||
} else if (size > 0) {
|
||||
|
|
@ -1251,7 +1247,7 @@ int UavcanServers::copyFw(const char *dst, const char *src)
|
|||
written = write(dfd, &buffer[total_written], remaining);
|
||||
|
||||
if (written < 0) {
|
||||
warnx("copyFw: couldn't write");
|
||||
PX4_ERR("copyFw: couldn't write");
|
||||
rv = -errno;
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -35,9 +35,11 @@
|
|||
|
||||
#include <px4_config.h>
|
||||
#include "uavcan_driver.hpp"
|
||||
#include <drivers/device/device.h>
|
||||
#include <drivers/device/Device.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/PublicationQueued.hpp>
|
||||
#include <uORB/topics/uavcan_parameter_value.h>
|
||||
#include <uORB/topics/vehicle_command_ack.h>
|
||||
|
||||
#include <uavcan/node/sub_node.hpp>
|
||||
|
|
@ -110,9 +112,9 @@ public:
|
|||
bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
|
||||
|
||||
private:
|
||||
pthread_t _subnode_thread;
|
||||
pthread_mutex_t _subnode_mutex;
|
||||
volatile bool _subnode_thread_should_exit = false;
|
||||
pthread_t _subnode_thread{-1};
|
||||
pthread_mutex_t _subnode_mutex{};
|
||||
volatile bool _subnode_thread_should_exit{false};
|
||||
|
||||
int init();
|
||||
|
||||
|
|
@ -161,7 +163,7 @@ private:
|
|||
bool _cmd_in_progress = false;
|
||||
|
||||
// uORB topic handle for MAVLink parameter responses
|
||||
orb_advert_t _param_response_pub = nullptr;
|
||||
uORB::Publication<uavcan_parameter_value_s> _param_response_pub{ORB_ID(uavcan_parameter_value)};
|
||||
uORB::PublicationQueued<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
|
|
@ -171,6 +173,7 @@ private:
|
|||
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);
|
||||
|
|
|
|||
|
|
@ -140,12 +140,23 @@ public:
|
|||
DeviceBusType_SIMULATION = 4,
|
||||
};
|
||||
|
||||
/**
|
||||
* Return the bus ID the device is connected to.
|
||||
*
|
||||
* @return The bus ID
|
||||
/*
|
||||
broken out device elements. The bitfields are used to keep
|
||||
the overall value small enough to fit in a float accurately,
|
||||
which makes it possible to transport over the MAVLink
|
||||
parameter protocol without loss of information.
|
||||
*/
|
||||
uint8_t get_device_bus() const { return _device_id.devid_s.bus; }
|
||||
struct DeviceStructure {
|
||||
DeviceBusType bus_type : 3;
|
||||
uint8_t bus: 5; // which instance of the bus type
|
||||
uint8_t address; // address on the bus (eg. I2C address)
|
||||
uint8_t devtype; // device class specific device type
|
||||
};
|
||||
|
||||
union DeviceId {
|
||||
struct DeviceStructure devid_s;
|
||||
uint32_t devid;
|
||||
};
|
||||
|
||||
uint32_t get_device_id() const { return _device_id.devid; }
|
||||
|
||||
|
|
@ -154,7 +165,8 @@ public:
|
|||
*
|
||||
* @return The bus type
|
||||
*/
|
||||
DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; }
|
||||
DeviceBusType get_device_bus_type() const { return _device_id.devid_s.bus_type; }
|
||||
void set_device_bus_type(DeviceBusType bus_type) { _device_id.devid_s.bus_type = bus_type; }
|
||||
|
||||
static const char *get_device_bus_string(DeviceBusType bus)
|
||||
{
|
||||
|
|
@ -177,41 +189,29 @@ public:
|
|||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the bus ID the device is connected to.
|
||||
*
|
||||
* @return The bus ID
|
||||
*/
|
||||
uint8_t get_device_bus() const { return _device_id.devid_s.bus; }
|
||||
void set_device_bus(uint8_t bus) { _device_id.devid_s.bus = bus; }
|
||||
|
||||
/**
|
||||
* Return the bus address of the device.
|
||||
*
|
||||
* @return The bus address
|
||||
*/
|
||||
uint8_t get_device_address() const { return _device_id.devid_s.address; }
|
||||
|
||||
void set_device_address(int address) { _device_id.devid_s.address = address; }
|
||||
uint8_t get_device_address() const { return _device_id.devid_s.address; }
|
||||
void set_device_address(int address) { _device_id.devid_s.address = address; }
|
||||
|
||||
/**
|
||||
* Set the device type
|
||||
* Return the device type
|
||||
*
|
||||
* @return The device type
|
||||
*/
|
||||
void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
|
||||
|
||||
virtual bool external() { return false; }
|
||||
|
||||
/*
|
||||
broken out device elements. The bitfields are used to keep
|
||||
the overall value small enough to fit in a float accurately,
|
||||
which makes it possible to transport over the MAVLink
|
||||
parameter protocol without loss of information.
|
||||
*/
|
||||
struct DeviceStructure {
|
||||
enum DeviceBusType bus_type : 3;
|
||||
uint8_t bus: 5; // which instance of the bus type
|
||||
uint8_t address; // address on the bus (eg. I2C address)
|
||||
uint8_t devtype; // device class specific device type
|
||||
};
|
||||
|
||||
union DeviceId {
|
||||
struct DeviceStructure devid_s;
|
||||
uint32_t devid;
|
||||
};
|
||||
uint8_t get_device_type() const { return _device_id.devid_s.devtype; }
|
||||
void set_device_type(uint8_t devtype) { _device_id.devid_s.devtype = devtype; }
|
||||
|
||||
/**
|
||||
* Print decoded device id string to a buffer.
|
||||
|
|
@ -234,30 +234,34 @@ public:
|
|||
return num_written;
|
||||
}
|
||||
|
||||
virtual bool external() const { return false; }
|
||||
|
||||
protected:
|
||||
union DeviceId _device_id {}; /**< device identifier information */
|
||||
union DeviceId _device_id {}; /**< device identifier information */
|
||||
|
||||
const char *_name{nullptr}; /**< driver name */
|
||||
bool _debug_enabled{false}; /**< if true, debug messages are printed */
|
||||
|
||||
explicit Device(const char *name) : _name(name)
|
||||
{
|
||||
/* setup a default device ID. When bus_type is UNKNOWN the
|
||||
other fields are invalid */
|
||||
_device_id.devid = 0;
|
||||
_device_id.devid_s.bus_type = DeviceBusType_UNKNOWN;
|
||||
_device_id.devid_s.bus = 0;
|
||||
_device_id.devid_s.address = 0;
|
||||
_device_id.devid_s.devtype = 0;
|
||||
set_device_bus_type(DeviceBusType_UNKNOWN);
|
||||
}
|
||||
|
||||
Device(const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
|
||||
: _name(name)
|
||||
{
|
||||
set_device_bus_type(bus_type);
|
||||
set_device_bus(bus);
|
||||
set_device_address(address);
|
||||
set_device_type(devtype);
|
||||
}
|
||||
|
||||
Device(DeviceBusType bus_type, uint8_t bus, uint8_t address, uint8_t devtype = 0)
|
||||
{
|
||||
_device_id.devid = 0;
|
||||
_device_id.devid_s.bus_type = bus_type;
|
||||
_device_id.devid_s.bus = bus;
|
||||
_device_id.devid_s.address = address;
|
||||
_device_id.devid_s.devtype = devtype;
|
||||
set_device_bus_type(bus_type);
|
||||
set_device_bus(bus);
|
||||
set_device_address(address);
|
||||
set_device_type(devtype);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ public:
|
|||
I2C(I2C &&) = delete;
|
||||
I2C &operator=(I2C &&) = delete;
|
||||
|
||||
virtual int init();
|
||||
virtual int init() override;
|
||||
|
||||
static int set_bus_clock(unsigned bus, unsigned clock_hz);
|
||||
|
||||
|
|
@ -109,7 +109,7 @@ protected:
|
|||
*/
|
||||
int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
|
||||
|
||||
bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
|
||||
virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); }
|
||||
|
||||
private:
|
||||
uint32_t _frequency{0};
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ protected:
|
|||
LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
|
||||
};
|
||||
|
||||
virtual int init();
|
||||
virtual int init() override;
|
||||
|
||||
/**
|
||||
* Check for the presence of the device on the bus.
|
||||
|
|
@ -164,7 +164,7 @@ protected:
|
|||
|
||||
int _transferhword(uint16_t *send, uint16_t *recv, unsigned len);
|
||||
|
||||
bool external() { return px4_spi_bus_external(get_device_bus()); }
|
||||
virtual bool external() const override { return px4_spi_bus_external(get_device_bus()); }
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -66,7 +66,7 @@ public:
|
|||
I2C(I2C &&) = delete;
|
||||
I2C &operator=(I2C &&) = delete;
|
||||
|
||||
virtual int init();
|
||||
virtual int init() override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
|
|
@ -106,7 +106,7 @@ protected:
|
|||
*/
|
||||
int transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const unsigned recv_len);
|
||||
|
||||
bool external() { return px4_i2c_bus_external(_device_id.devid_s.bus); }
|
||||
virtual bool external() const override { return px4_i2c_bus_external(_device_id.devid_s.bus); }
|
||||
|
||||
private:
|
||||
int _fd{-1};
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ protected:
|
|||
LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */
|
||||
};
|
||||
|
||||
virtual int init();
|
||||
virtual int init() override;
|
||||
|
||||
/**
|
||||
* Check for the presence of the device on the bus.
|
||||
|
|
@ -179,7 +179,7 @@ protected:
|
|||
|
||||
int _transferhword(uint16_t *send, uint16_t *recv, unsigned len);
|
||||
|
||||
bool external() { return px4_spi_bus_external(get_device_bus()); }
|
||||
virtual bool external() const override { return px4_spi_bus_external(get_device_bus()); }
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -92,7 +92,7 @@
|
|||
<min>0</min>
|
||||
<max>4000</max>
|
||||
</parameter>
|
||||
<parameter default="0.0" name="mot_ls" type="FLOAT">
|
||||
<parameter default="0.0" name="mot_ls" type="FLOAT">
|
||||
<short_desc>READ ONLY: Motor inductance in henries.</short_desc>
|
||||
<long_desc>READ ONLY: Motor inductance in henries. This is measured on start-up.</long_desc>
|
||||
<unit>henries</unit>
|
||||
|
|
@ -135,4 +135,66 @@
|
|||
<min>0</min>
|
||||
</parameter>
|
||||
</group>
|
||||
</parameters>
|
||||
<group name="UAVCAN GNSS" no_code_generation="true">
|
||||
<parameter name="gnss.warn_dimens" default="0" type="INT32">
|
||||
<short_desc>device health warning</short_desc>
|
||||
<long_desc>Set the device health to Warning if the dimensionality of
|
||||
the GNSS solution is less than this value. 3 for the full (3D)
|
||||
solution, 2 for planar (2D) solution, 1 for time-only solution,
|
||||
0 disables the feature.
|
||||
</long_desc>
|
||||
<min>0</min>
|
||||
<max>3</max>
|
||||
<values>
|
||||
<value code="0">disables the feature</value>
|
||||
<value code="1">time-only solution</value>
|
||||
<value code="2">planar (2D) solution</value>
|
||||
<value code="3">full (3D) solution</value>
|
||||
</values>
|
||||
</parameter>
|
||||
<parameter name="gnss.warn_sats" default="0" type="INT32">
|
||||
<short_desc></short_desc>
|
||||
<long_desc>Set the device health to Warning if the number of satellites
|
||||
used in the GNSS solution is below this threshold. Zero
|
||||
disables the feature
|
||||
</long_desc>
|
||||
</parameter>
|
||||
<parameter name="gnss.dyn_model" default="2" type="INT32">
|
||||
<short_desc>GNSS dynamic model</short_desc>
|
||||
<long_desc>Dynamic model used in the GNSS positioning engine. 0 –
|
||||
Automotive, 1 – Sea, 2 – Airborne.
|
||||
</long_desc>
|
||||
<min>0</min>
|
||||
<max>2</max>
|
||||
<values>
|
||||
<value code="0">Automotive</value>
|
||||
<value code="1">Sea</value>
|
||||
<value code="2">Airborne</value>
|
||||
</values>
|
||||
</parameter>
|
||||
<parameter name="gnss.old_fix_msg" default="1" type="INT32">
|
||||
<short_desc>Broadcast old GNSS fix message</short_desc>
|
||||
<long_desc>Broadcast the old (deprecated) GNSS fix message
|
||||
uavcan.equipment.gnss.Fix alongside the new alternative
|
||||
uavcan.equipment.gnss.Fix2. It is recommended to
|
||||
disable this feature to reduce the CAN bus traffic.
|
||||
</long_desc>
|
||||
<min>0</min>
|
||||
<max>1</max>
|
||||
<values>
|
||||
<value code="0">Fix2</value>
|
||||
<value code="1">Fix and Fix2</value>
|
||||
</values>
|
||||
</parameter>
|
||||
<parameter name="uavcan.pubp-pres" default="0" type="INT32">
|
||||
<short_desc></short_desc>
|
||||
<long_desc>Set the device health to Warning if the number of satellites
|
||||
used in the GNSS solution is below this threshold. Zero
|
||||
disables the feature
|
||||
</long_desc>
|
||||
<unit>microseconds</unit>
|
||||
<min>0</min>
|
||||
<max>1000000</max>
|
||||
</parameter>
|
||||
</group>
|
||||
</parameters>
|
||||
|
|
|
|||
Loading…
Reference in New Issue