mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
uavcan start enforcing code style (#7856)
This commit is contained in:
Daniel Agar
GitHub
parent
de6a552b53
commit
edea4a369e
@ -15,12 +15,9 @@ exec find src \
|
||||
-path src/lib/ecl -prune -o \
|
||||
-path src/lib/matrix -prune -o \
|
||||
-path src/lib/micro-CDR -prune -o \
|
||||
-path src/modules/micrortps_bridge/micrortps_agent -prune -o \
|
||||
-path src/modules/commander -prune -o \
|
||||
-path src/modules/micrortps_bridge/micrortps_agent -prune -o \
|
||||
-path src/modules/sdlog2 -prune -o \
|
||||
-path src/modules/systemlib/uthash -prune -o \
|
||||
-path src/modules/uavcan -prune -o \
|
||||
-path src/modules/uavcan/libuavcan -prune -o \
|
||||
-path src/modules/uavcanesc -o \
|
||||
-path src/modules/uavcannode -o \
|
||||
-type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
|
||||
|
||||
@ -168,9 +168,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
|
||||
(void)_uavcan_pub_raw_cmd.broadcast(msg);
|
||||
|
||||
// Publish actuator outputs
|
||||
if (_actuator_outputs_pub != nullptr)
|
||||
{
|
||||
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,
|
||||
|
||||
@ -45,14 +45,12 @@
|
||||
namespace uavcan_node
|
||||
{
|
||||
|
||||
struct AllocatorSynchronizer
|
||||
{
|
||||
struct AllocatorSynchronizer {
|
||||
const ::irqstate_t state = ::enter_critical_section();
|
||||
~AllocatorSynchronizer() { ::leave_critical_section(state); }
|
||||
};
|
||||
|
||||
struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>
|
||||
{
|
||||
struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer> {
|
||||
static constexpr unsigned CapacitySoftLimit = 250;
|
||||
static constexpr unsigned CapacityHardLimit = 500;
|
||||
|
||||
@ -62,11 +60,10 @@ struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSiz
|
||||
|
||||
~Allocator()
|
||||
{
|
||||
if (getNumAllocatedBlocks() > 0)
|
||||
{
|
||||
warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
}
|
||||
if (getNumAllocatedBlocks() > 0) {
|
||||
warnx("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -64,18 +64,21 @@ UavcanGnssBridge::~UavcanGnssBridge()
|
||||
int UavcanGnssBridge::init()
|
||||
{
|
||||
int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix2 pub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix sub failed %i", res);
|
||||
return res;
|
||||
}
|
||||
|
||||
res = _sub_fix2.start(Fix2CbBinder(this, &UavcanGnssBridge::gnss_fix2_sub_cb));
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS fix2 sub failed %i", res);
|
||||
return res;
|
||||
@ -95,7 +98,7 @@ unsigned UavcanGnssBridge::get_num_redundant_channels() 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));
|
||||
_sub_fix.getFailureCount(), int(_old_fix_subscriber_active));
|
||||
|
||||
if (_receiver_node_id < 0) {
|
||||
printf("N/A\n");
|
||||
@ -128,36 +131,36 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
||||
_receiver_node_id = -1;
|
||||
}
|
||||
|
||||
float pos_cov[9]{};
|
||||
float vel_cov[9]{};
|
||||
float pos_cov[9] {};
|
||||
float vel_cov[9] {};
|
||||
bool valid_covariances = true;
|
||||
|
||||
switch (msg.covariance.size()) {
|
||||
// Scalar matrix
|
||||
case 1: {
|
||||
const auto x = msg.covariance[0];
|
||||
const auto x = msg.covariance[0];
|
||||
|
||||
pos_cov[0] = x;
|
||||
pos_cov[4] = x;
|
||||
pos_cov[8] = x;
|
||||
pos_cov[0] = x;
|
||||
pos_cov[4] = x;
|
||||
pos_cov[8] = x;
|
||||
|
||||
vel_cov[0] = x;
|
||||
vel_cov[4] = x;
|
||||
vel_cov[8] = x;
|
||||
break;
|
||||
}
|
||||
vel_cov[0] = x;
|
||||
vel_cov[4] = x;
|
||||
vel_cov[8] = x;
|
||||
break;
|
||||
}
|
||||
|
||||
// Diagonal matrix (the most common case)
|
||||
case 6: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[4] = msg.covariance[1];
|
||||
pos_cov[8] = msg.covariance[2];
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[4] = msg.covariance[1];
|
||||
pos_cov[8] = msg.covariance[2];
|
||||
|
||||
vel_cov[0] = msg.covariance[3];
|
||||
vel_cov[4] = msg.covariance[4];
|
||||
vel_cov[8] = msg.covariance[5];
|
||||
break;
|
||||
}
|
||||
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.
|
||||
@ -169,26 +172,27 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
||||
// 16 18 19
|
||||
// 17 19 20
|
||||
case 21: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[1];
|
||||
pos_cov[4] = msg.covariance[6];
|
||||
pos_cov[5] = msg.covariance[7];
|
||||
pos_cov[6] = msg.covariance[2];
|
||||
pos_cov[7] = msg.covariance[7];
|
||||
pos_cov[8] = msg.covariance[11];
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[1];
|
||||
pos_cov[4] = msg.covariance[6];
|
||||
pos_cov[5] = msg.covariance[7];
|
||||
pos_cov[6] = msg.covariance[2];
|
||||
pos_cov[7] = msg.covariance[7];
|
||||
pos_cov[8] = msg.covariance[11];
|
||||
|
||||
vel_cov[0] = msg.covariance[15];
|
||||
vel_cov[1] = msg.covariance[16];
|
||||
vel_cov[2] = msg.covariance[17];
|
||||
vel_cov[3] = msg.covariance[16];
|
||||
vel_cov[4] = msg.covariance[18];
|
||||
vel_cov[5] = msg.covariance[19];
|
||||
vel_cov[6] = msg.covariance[17];
|
||||
vel_cov[7] = msg.covariance[19];
|
||||
vel_cov[8] = msg.covariance[20];
|
||||
}
|
||||
|
||||
vel_cov[0] = msg.covariance[15];
|
||||
vel_cov[1] = msg.covariance[16];
|
||||
vel_cov[2] = msg.covariance[17];
|
||||
vel_cov[3] = msg.covariance[16];
|
||||
vel_cov[4] = msg.covariance[18];
|
||||
vel_cov[5] = msg.covariance[19];
|
||||
vel_cov[6] = msg.covariance[17];
|
||||
vel_cov[7] = msg.covariance[19];
|
||||
vel_cov[8] = msg.covariance[20];
|
||||
}
|
||||
/* FALLTHROUGH */
|
||||
|
||||
// Full matrix 6x6.
|
||||
@ -201,33 +205,34 @@ void UavcanGnssBridge::gnss_fix2_sub_cb(const uavcan::ReceivedDataStructure<uavc
|
||||
// 27 28 29
|
||||
// 33 34 35
|
||||
case 36: {
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[6];
|
||||
pos_cov[4] = msg.covariance[7];
|
||||
pos_cov[5] = msg.covariance[8];
|
||||
pos_cov[6] = msg.covariance[12];
|
||||
pos_cov[7] = msg.covariance[13];
|
||||
pos_cov[8] = msg.covariance[14];
|
||||
pos_cov[0] = msg.covariance[0];
|
||||
pos_cov[1] = msg.covariance[1];
|
||||
pos_cov[2] = msg.covariance[2];
|
||||
pos_cov[3] = msg.covariance[6];
|
||||
pos_cov[4] = msg.covariance[7];
|
||||
pos_cov[5] = msg.covariance[8];
|
||||
pos_cov[6] = msg.covariance[12];
|
||||
pos_cov[7] = msg.covariance[13];
|
||||
pos_cov[8] = msg.covariance[14];
|
||||
|
||||
vel_cov[0] = msg.covariance[21];
|
||||
vel_cov[1] = msg.covariance[22];
|
||||
vel_cov[2] = msg.covariance[23];
|
||||
vel_cov[3] = msg.covariance[27];
|
||||
vel_cov[4] = msg.covariance[28];
|
||||
vel_cov[5] = msg.covariance[29];
|
||||
vel_cov[6] = msg.covariance[33];
|
||||
vel_cov[7] = msg.covariance[34];
|
||||
vel_cov[8] = msg.covariance[35];
|
||||
}
|
||||
|
||||
vel_cov[0] = msg.covariance[21];
|
||||
vel_cov[1] = msg.covariance[22];
|
||||
vel_cov[2] = msg.covariance[23];
|
||||
vel_cov[3] = msg.covariance[27];
|
||||
vel_cov[4] = msg.covariance[28];
|
||||
vel_cov[5] = msg.covariance[29];
|
||||
vel_cov[6] = msg.covariance[33];
|
||||
vel_cov[7] = msg.covariance[34];
|
||||
vel_cov[8] = msg.covariance[35];
|
||||
}
|
||||
/* FALLTHROUGH */
|
||||
|
||||
// Either empty or invalid sized, interpret as zero matrix
|
||||
default: {
|
||||
valid_covariances = false;
|
||||
break; // Nothing to do
|
||||
}
|
||||
valid_covariances = false;
|
||||
break; // Nothing to do
|
||||
}
|
||||
}
|
||||
|
||||
process_fixx(msg, pos_cov, vel_cov, valid_covariances, valid_covariances);
|
||||
@ -235,10 +240,10 @@ 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) {
|
||||
@ -314,8 +319,8 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
||||
report.vel_e_m_s = msg.ned_velocity[1];
|
||||
report.vel_d_m_s = msg.ned_velocity[2];
|
||||
report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s +
|
||||
report.vel_e_m_s * report.vel_e_m_s +
|
||||
report.vel_d_m_s * report.vel_d_m_s);
|
||||
report.vel_e_m_s * report.vel_e_m_s +
|
||||
report.vel_d_m_s * report.vel_d_m_s);
|
||||
report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
|
||||
report.vel_ned_valid = true;
|
||||
|
||||
@ -325,27 +330,29 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
||||
|
||||
switch (msg.gnss_time_standard) {
|
||||
case FixType::GNSS_TIME_STANDARD_UTC: {
|
||||
report.time_utc_usec = gnss_ts_usec;
|
||||
break;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
if (msg.num_leap_seconds > 0) {
|
||||
report.time_utc_usec = gnss_ts_usec - msg.num_leap_seconds - 10;
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//TODO px4_clock_settime does nothing on the Snapdragon platform
|
||||
@ -375,7 +382,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
|
||||
// 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);
|
||||
ORB_PRIO_HIGH);
|
||||
|
||||
// Doing less time critical stuff here
|
||||
if (_orb_to_uavcan_pub_timer.isRunning()) {
|
||||
@ -389,16 +396,19 @@ void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
|
||||
if (_orb_sub_gnss < 0) {
|
||||
// ORB subscription stops working if this is relocated into init()
|
||||
_orb_sub_gnss = orb_subscribe(ORB_ID(vehicle_gps_position));
|
||||
|
||||
if (_orb_sub_gnss < 0) {
|
||||
PX4_WARN("GNSS ORB sub errno %d", errno);
|
||||
return;
|
||||
}
|
||||
|
||||
PX4_WARN("GNSS ORB fd %d", _orb_sub_gnss);
|
||||
}
|
||||
|
||||
{
|
||||
bool updated = false;
|
||||
const int res = orb_check(_orb_sub_gnss, &updated);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS ORB check err %d %d", res, errno);
|
||||
return;
|
||||
@ -411,6 +421,7 @@ void UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &)
|
||||
|
||||
auto orb_msg = ::vehicle_gps_position_s();
|
||||
const int res = orb_copy(ORB_ID(vehicle_gps_position), _orb_sub_gnss, &orb_msg);
|
||||
|
||||
if (res < 0) {
|
||||
PX4_WARN("GNSS ORB read errno %d", errno);
|
||||
return;
|
||||
|
||||
@ -80,10 +80,10 @@ private:
|
||||
|
||||
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 &);
|
||||
|
||||
|
||||
@ -142,7 +142,8 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
|
||||
void UavcanMagnetometerBridge::mag_sub_cb(const
|
||||
uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::MagneticFieldStrength>
|
||||
&msg)
|
||||
{
|
||||
lock();
|
||||
|
||||
@ -121,6 +121,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
|
||||
channel->class_instance = class_instance;
|
||||
|
||||
channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_VERY_HIGH);
|
||||
|
||||
if (channel->orb_advert == nullptr) {
|
||||
DEVICE_LOG("ADVERTISE FAILED");
|
||||
(void)unregister_class_devname(_class_devname, class_instance);
|
||||
|
||||
@ -104,11 +104,12 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
|
||||
std::abort();
|
||||
}
|
||||
|
||||
res = px4_sem_init(&_server_command_sem, 0 , 0);
|
||||
res = px4_sem_init(&_server_command_sem, 0, 0);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
|
||||
/* _server_command_sem use case is a signal */
|
||||
px4_sem_setprotocol(&_server_command_sem, SEM_PRIO_NONE);
|
||||
}
|
||||
@ -541,7 +542,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
* Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
|
||||
* shipped with libuavcan does not support deinitialization.
|
||||
*/
|
||||
static CanInitHelper* can = nullptr;
|
||||
static CanInitHelper *can = nullptr;
|
||||
|
||||
if (can == nullptr) {
|
||||
|
||||
@ -1120,22 +1121,24 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
break;
|
||||
|
||||
case UAVCAN_IOCG_NODEID_INPROGRESS: {
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
UavcanServers *_servers = UavcanServers::instance();
|
||||
|
||||
if (_servers == nullptr) {
|
||||
// status unavailable
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
} else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
|
||||
// node discovery complete
|
||||
ret = -ETIME;
|
||||
break;
|
||||
} else {
|
||||
// node discovery in progress
|
||||
ret = OK;
|
||||
break;
|
||||
if (_servers == nullptr) {
|
||||
// status unavailable
|
||||
ret = -EINVAL;
|
||||
break;
|
||||
|
||||
} else if (_servers->guessIfAllDynamicNodesAreAllocated()) {
|
||||
// node discovery complete
|
||||
ret = -ETIME;
|
||||
break;
|
||||
|
||||
} else {
|
||||
// node discovery in progress
|
||||
ret = OK;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
@ -1212,7 +1215,7 @@ UavcanNode::print_info()
|
||||
|
||||
for (uint8_t i = 0; i < _outputs.noutputs; i++) {
|
||||
const float temp_celsius = (esc.esc[i].esc_temperature > 0) ?
|
||||
(esc.esc[i].esc_temperature - 273.15F) : 0.0F;
|
||||
(esc.esc[i].esc_temperature - 273.15F) : 0.0F;
|
||||
|
||||
printf("%d\t", esc.esc[i].esc_address);
|
||||
printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
|
||||
@ -1240,10 +1243,10 @@ UavcanNode::print_info()
|
||||
// Printing all nodes that are online
|
||||
std::printf("Online nodes (Node ID, Health, Mode):\n");
|
||||
_node_status_monitor.forEachNode([](uavcan::NodeID nid, uavcan::NodeStatusMonitor::NodeStatus ns) {
|
||||
static constexpr const char* HEALTH[] = {
|
||||
static constexpr const char *HEALTH[] = {
|
||||
"OK", "WARN", "ERR", "CRIT"
|
||||
};
|
||||
static constexpr const char* MODES[] = {
|
||||
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]);
|
||||
@ -1427,12 +1430,15 @@ int uavcan_main(int argc, char *argv[])
|
||||
if (hardpoint_id >= 0 && hardpoint_id < 256 &&
|
||||
command >= 0 && command < 65536) {
|
||||
inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command);
|
||||
|
||||
} else {
|
||||
errx(1, "Invalid argument");
|
||||
}
|
||||
|
||||
} else {
|
||||
errx(1, "Invalid hardpoint command");
|
||||
}
|
||||
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
@ -1444,10 +1450,12 @@ int uavcan_main(int argc, char *argv[])
|
||||
/* Let's recover any memory we can */
|
||||
|
||||
inst->shrink();
|
||||
|
||||
if (rv < 0) {
|
||||
warnx("Firmware Server Failed to Stop %d", rv);
|
||||
::exit(rv);
|
||||
}
|
||||
|
||||
::exit(0);
|
||||
|
||||
} else {
|
||||
|
||||
@ -104,7 +104,7 @@ class UavcanNode : public device::CDev
|
||||
|
||||
public:
|
||||
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
enum eServerAction {None, Start, Stop, CheckFW , Busy};
|
||||
enum eServerAction {None, Start, Stop, CheckFW, Busy};
|
||||
|
||||
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
|
||||
|
||||
|
||||
@ -169,6 +169,7 @@ int UavcanServers::start(uavcan::INode &main_node)
|
||||
(void)pthread_attr_getschedparam(&tattr, ¶m);
|
||||
tattr.stacksize = PX4_STACK_ADJUSTED(StackSize);
|
||||
param.sched_priority = Priority;
|
||||
|
||||
if (pthread_attr_setschedparam(&tattr, ¶m)) {
|
||||
warnx("setting sched params failed");
|
||||
}
|
||||
@ -324,6 +325,7 @@ 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);
|
||||
}
|
||||
@ -343,31 +345,40 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
*/
|
||||
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) {
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
|
||||
if (request.param_index >= 0) {
|
||||
req.index = request.param_index;
|
||||
|
||||
} else {
|
||||
req.name = (char*)request.param_id;
|
||||
req.name = (char *)request.param_id;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
_param_index = request.param_index;
|
||||
}
|
||||
|
||||
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) {
|
||||
uavcan::protocol::param::GetSet::Request req;
|
||||
|
||||
if (request.param_index >= 0) {
|
||||
req.index = request.param_index;
|
||||
|
||||
} else {
|
||||
req.name = (char*)request.param_id;
|
||||
req.name = (char *)request.param_id;
|
||||
}
|
||||
|
||||
if (request.param_type == MAV_PARAM_TYPE_REAL32) {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::real_value>() = request.real_value;
|
||||
|
||||
} else if (request.param_type == MAV_PARAM_TYPE_UINT8) {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::boolean_value>() = request.int_value;
|
||||
|
||||
} else {
|
||||
req.value.to<uavcan::protocol::param::Value::Tag::integer_value>() = request.int_value;
|
||||
}
|
||||
@ -376,12 +387,15 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
set_node_params_dirty(request.node_id);
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
_param_index = request.param_index;
|
||||
}
|
||||
|
||||
} else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||
// This triggers the _param_list_in_progress case below.
|
||||
_param_index = 0;
|
||||
@ -391,6 +405,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
|
||||
warnx("UAVCAN command bridge: starting component-specific param list");
|
||||
}
|
||||
|
||||
} else if (request.node_id == MAV_COMP_ID_ALL) {
|
||||
if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {
|
||||
/*
|
||||
@ -408,6 +423,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
param_count(_param_list_node_id);
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
/*
|
||||
* Need to know how many parameters this node has before we can
|
||||
@ -427,8 +443,10 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
if (_param_list_all_nodes) {
|
||||
// We're listing all parameters for all nodes -- get the next node ID
|
||||
uint8_t next_id = get_next_active_node_id(_param_list_node_id);
|
||||
|
||||
if (next_id < 128) {
|
||||
_param_list_node_id = next_id;
|
||||
|
||||
/*
|
||||
* If there is a next node ID, check if that node's parameters
|
||||
* have been counted before. If not, do it now.
|
||||
@ -436,6 +454,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
if (_param_counts[_param_list_node_id] == 0) {
|
||||
param_count(_param_list_node_id);
|
||||
}
|
||||
|
||||
// Keep on listing.
|
||||
_param_index = 0;
|
||||
_param_list_in_progress = true;
|
||||
@ -453,9 +472,11 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
req.index = _param_index;
|
||||
|
||||
int call_res = _param_getset_client.call(_param_list_node_id, req);
|
||||
|
||||
if (call_res < 0) {
|
||||
_param_list_in_progress = false;
|
||||
warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res);
|
||||
|
||||
} else {
|
||||
_param_in_progress = true;
|
||||
}
|
||||
@ -490,21 +511,26 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
req.parameter_name = "esc_index";
|
||||
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
|
||||
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);
|
||||
beep(BeepFrequencyError);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
|
||||
|
||||
} else {
|
||||
beep(BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
warnx("UAVCAN command bridge: unknown command ID %d", command_id);
|
||||
cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
|
||||
int command_id = static_cast<int>(cmd.param1 + 0.5f);
|
||||
|
||||
@ -515,18 +541,23 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
// Param save request
|
||||
int node_id;
|
||||
node_id = get_next_dirty_node_id(1);
|
||||
|
||||
if (node_id < 128) {
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
|
||||
param_opcode(node_id);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case 2: {
|
||||
// Command is a param erase request -- apply it to all active nodes by setting the dirty bit
|
||||
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
|
||||
|
||||
for (int i = 1; i < 128; i = get_next_active_node_id(i)) {
|
||||
set_node_params_dirty(i);
|
||||
}
|
||||
|
||||
param_opcode(get_next_dirty_node_id(1));
|
||||
break;
|
||||
}
|
||||
@ -547,6 +578,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
|
||||
if (_command_ack_pub == nullptr) {
|
||||
_command_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &ack, vehicle_command_ack_s::ORB_QUEUE_LENGTH);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(vehicle_command_ack), _command_ack_pub, &ack);
|
||||
}
|
||||
@ -557,6 +589,7 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
|
||||
// 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);
|
||||
@ -587,6 +620,7 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
|
||||
if (result.isSuccessful()) {
|
||||
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
|
||||
|
||||
if (resp.name.size()) {
|
||||
_count_index++;
|
||||
_param_counts[node_id] = _count_index;
|
||||
@ -595,24 +629,28 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
req.index = _count_index;
|
||||
|
||||
int call_res = _param_getset_client.call(result.getCallID().server_node_id, req);
|
||||
|
||||
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);
|
||||
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]);
|
||||
beep(BeepFrequencyGenericIndication);
|
||||
}
|
||||
|
||||
} else {
|
||||
_param_counts[node_id] = 0;
|
||||
_count_in_progress = false;
|
||||
_count_index = 0;
|
||||
warnx("UAVCAN command bridge: GetSet error during param count");
|
||||
}
|
||||
|
||||
} else {
|
||||
/*
|
||||
* Currently in parameter get/set mode:
|
||||
@ -632,9 +670,11 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) {
|
||||
response.param_type = MAV_PARAM_TYPE_INT64;
|
||||
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::integer_value>();
|
||||
|
||||
} else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) {
|
||||
response.param_type = MAV_PARAM_TYPE_REAL32;
|
||||
response.real_value = param.value.to<uavcan::protocol::param::Value::Tag::real_value>();
|
||||
|
||||
} else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) {
|
||||
response.param_type = MAV_PARAM_TYPE_UINT8;
|
||||
response.int_value = param.value.to<uavcan::protocol::param::Value::Tag::boolean_value>();
|
||||
@ -642,9 +682,11 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: GetSet error");
|
||||
}
|
||||
@ -659,8 +701,10 @@ 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);
|
||||
|
||||
} else {
|
||||
_count_in_progress = true;
|
||||
_count_index = 0;
|
||||
@ -673,8 +717,10 @@ 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);
|
||||
|
||||
} else {
|
||||
_cmd_in_progress = true;
|
||||
warnx("UAVCAN command bridge: sent ExecuteOpcode");
|
||||
@ -691,16 +737,20 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("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);
|
||||
|
||||
} else {
|
||||
warnx("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);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN command bridge: sent RestartNode");
|
||||
_cmd_in_progress = true;
|
||||
@ -715,6 +765,7 @@ void UavcanServers::cb_opcode(const uavcan::ServiceCallResult<uavcan::protocol::
|
||||
* save/erase attempt will occur when the next save/erase command is received over MAVLink.
|
||||
*/
|
||||
node_id = get_next_dirty_node_id(node_id);
|
||||
|
||||
if (node_id < 128) {
|
||||
param_opcode(node_id);
|
||||
}
|
||||
@ -733,12 +784,14 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
|
||||
warnx("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);
|
||||
}
|
||||
|
||||
// Get the next dirty node ID and send the same command to it
|
||||
node_id = get_next_dirty_node_id(node_id);
|
||||
|
||||
if (node_id < 128) {
|
||||
param_opcode(node_id);
|
||||
}
|
||||
@ -747,15 +800,19 @@ void UavcanServers::cb_restart(const uavcan::ServiceCallResult<uavcan::protocol:
|
||||
uint8_t UavcanServers::get_next_active_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) ||
|
||||
_subnode.getNodeID().get() == base); base++);
|
||||
_subnode.getNodeID().get() == base); base++);
|
||||
|
||||
return base;
|
||||
}
|
||||
|
||||
uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base)
|
||||
{
|
||||
base++;
|
||||
|
||||
for (; base < 128 && !are_node_params_dirty(base); base++);
|
||||
|
||||
return base;
|
||||
}
|
||||
|
||||
@ -773,8 +830,11 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan:
|
||||
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("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);
|
||||
warnx("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());
|
||||
@ -789,17 +849,21 @@ void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult<uavcan:
|
||||
req.timeout_sec = _esc_enumeration_active ? 65535 : 0;
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent Begin request");
|
||||
}
|
||||
|
||||
} else {
|
||||
warnx("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");
|
||||
@ -811,6 +875,7 @@ void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructur
|
||||
|
||||
// First, check if we've already seen an indication from this ESC. If so, just ignore this indication.
|
||||
int i = 0;
|
||||
|
||||
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);
|
||||
@ -819,12 +884,15 @@ void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructur
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i);
|
||||
}
|
||||
@ -834,6 +902,7 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan
|
||||
{
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("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());
|
||||
|
||||
@ -847,10 +916,13 @@ void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult<uavcan
|
||||
uavcan::protocol::param::ExecuteOpcode::Request opcode_req;
|
||||
opcode_req.opcode = opcode_req.OPCODE_SAVE;
|
||||
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);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index);
|
||||
warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index],
|
||||
esc_index);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -864,9 +936,11 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::
|
||||
if (!result.isSuccessful()) {
|
||||
warnx("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());
|
||||
beep(BeepFrequencyError);
|
||||
|
||||
} else {
|
||||
warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get());
|
||||
beep(this_is_the_last_one ? BeepFrequencySuccess : BeepFrequencyGenericIndication);
|
||||
@ -884,15 +958,18 @@ void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::
|
||||
req.parameter_name = "esc_index";
|
||||
req.timeout_sec = 0;
|
||||
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);
|
||||
|
||||
} else {
|
||||
warnx("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
|
||||
overriding any firmware the user has already loaded there.
|
||||
@ -936,7 +1013,8 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
char dstpath[maxlen + 1];
|
||||
char srcpath[maxlen + 1];
|
||||
|
||||
DIR* const romfs_dir = opendir(romfs_path);
|
||||
DIR *const romfs_dir = opendir(romfs_path);
|
||||
|
||||
if (!romfs_dir) {
|
||||
return;
|
||||
}
|
||||
@ -945,7 +1023,8 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
memcpy(srcpath, romfs_path, romfs_path_len + 1);
|
||||
|
||||
// Iterate over all device directories in ROMFS
|
||||
struct dirent* dev_dirent = NULL;
|
||||
struct dirent *dev_dirent = NULL;
|
||||
|
||||
while ((dev_dirent = readdir(romfs_dir)) != NULL) {
|
||||
// Skip if not a directory
|
||||
if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) {
|
||||
@ -955,11 +1034,14 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
// Make sure the path fits
|
||||
size_t dev_dirname_len = strlen(dev_dirent->d_name);
|
||||
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);
|
||||
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);
|
||||
continue;
|
||||
@ -971,6 +1053,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
|
||||
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
|
||||
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("dev: couldn't create '%s'", dstpath);
|
||||
continue;
|
||||
@ -981,14 +1064,16 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
srcpath[romfs_path_len] = '/';
|
||||
memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1);
|
||||
|
||||
DIR* const dev_dir = opendir(srcpath);
|
||||
DIR *const dev_dir = opendir(srcpath);
|
||||
|
||||
if (!dev_dir) {
|
||||
warnx("dev: couldn't open '%s'", srcpath);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Iterate over all version directories in the current ROMFS device directory
|
||||
struct dirent* ver_dirent = NULL;
|
||||
struct dirent *ver_dirent = NULL;
|
||||
|
||||
while ((ver_dirent = readdir(dev_dir)) != NULL) {
|
||||
// Skip if not a directory
|
||||
if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) {
|
||||
@ -998,11 +1083,14 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
// Make sure the path fits
|
||||
size_t ver_dirname_len = strlen(ver_dirent->d_name);
|
||||
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);
|
||||
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);
|
||||
continue;
|
||||
@ -1014,6 +1102,7 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
|
||||
if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
|
||||
rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("ver: couldn't create '%s'", dstpath);
|
||||
continue;
|
||||
@ -1026,15 +1115,17 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
|
||||
// Find the name of the bundled firmware file, or move on to the
|
||||
// next directory if there's no file here.
|
||||
DIR* const src_ver_dir = opendir(srcpath);
|
||||
DIR *const src_ver_dir = opendir(srcpath);
|
||||
|
||||
if (!src_ver_dir) {
|
||||
warnx("ver: couldn't open '%s'", srcpath);
|
||||
continue;
|
||||
}
|
||||
|
||||
struct dirent* src_fw_dirent = NULL;
|
||||
struct dirent *src_fw_dirent = NULL;
|
||||
|
||||
while ((src_fw_dirent = readdir(src_ver_dir)) != NULL &&
|
||||
!DIRENT_ISFILE(src_fw_dirent->d_type));
|
||||
!DIRENT_ISFILE(src_fw_dirent->d_type));
|
||||
|
||||
if (!src_fw_dirent) {
|
||||
(void)closedir(src_ver_dir);
|
||||
@ -1046,11 +1137,14 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
bool copy_fw = true;
|
||||
|
||||
// Clear out any romfs_ files in the version directory on the SD card
|
||||
DIR* const dst_ver_dir = opendir(dstpath);
|
||||
DIR *const dst_ver_dir = opendir(dstpath);
|
||||
|
||||
if (!dst_ver_dir) {
|
||||
warnx("unlink: couldn't open '%s'", dstpath);
|
||||
|
||||
} else {
|
||||
struct dirent* fw_dirent = NULL;
|
||||
struct dirent *fw_dirent = NULL;
|
||||
|
||||
while ((fw_dirent = readdir(dst_ver_dir)) != NULL) {
|
||||
// Skip if not a file
|
||||
if (!DIRENT_ISFILE(fw_dirent->d_type)) {
|
||||
@ -1063,12 +1157,15 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
* so don't bother deleting and rewriting it.
|
||||
*/
|
||||
copy_fw = false;
|
||||
|
||||
} else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) {
|
||||
size_t dst_fw_len = strlen(fw_dirent->d_name);
|
||||
size_t dstpath_fw_len = dstpath_ver_len + dst_fw_len;
|
||||
|
||||
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);
|
||||
|
||||
} else {
|
||||
// File name starts with "_", delete it.
|
||||
dstpath[dstpath_ver_len] = '/';
|
||||
@ -1076,11 +1173,13 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
unlink(dstpath);
|
||||
warnx("unlink: removed '%s'", dstpath);
|
||||
}
|
||||
|
||||
} else {
|
||||
// User file, don't copy firmware
|
||||
copy_fw = false;
|
||||
}
|
||||
}
|
||||
|
||||
(void)closedir(dst_ver_dir);
|
||||
}
|
||||
|
||||
@ -1091,19 +1190,23 @@ void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_pat
|
||||
|
||||
if (srcpath_fw_len > maxlen) {
|
||||
warnx("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);
|
||||
|
||||
} else {
|
||||
// All OK, make the paths and copy the file
|
||||
srcpath[srcpath_ver_len] = '/';
|
||||
memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
|
||||
|
||||
dstpath[dstpath_ver_len] = '/';
|
||||
memcpy(&dstpath[dstpath_ver_len +1], src_fw_dirent->d_name, fw_len + 1);
|
||||
memcpy(&dstpath[dstpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1);
|
||||
|
||||
rv = copyFw(dstpath, srcpath);
|
||||
|
||||
if (rv != 0) {
|
||||
warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv);
|
||||
|
||||
} else {
|
||||
warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath);
|
||||
}
|
||||
@ -1119,18 +1222,21 @@ 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];
|
||||
|
||||
dfd = open(dst, O_WRONLY | O_CREAT, 0666);
|
||||
|
||||
if (dfd < 0) {
|
||||
warnx("copyFw: couldn't open dst");
|
||||
return -errno;
|
||||
}
|
||||
|
||||
sfd = open(src, O_RDONLY, 0);
|
||||
|
||||
if (sfd < 0) {
|
||||
(void)close(dfd);
|
||||
warnx("copyFw: couldn't open src");
|
||||
@ -1138,21 +1244,27 @@ int UavcanServers::copyFw(const char* dst, const char* src) {
|
||||
}
|
||||
|
||||
ssize_t size = 0;
|
||||
|
||||
do {
|
||||
size = read(sfd, buffer, sizeof(buffer));
|
||||
|
||||
if (size < 0) {
|
||||
warnx("copyFw: couldn't read");
|
||||
rv = -errno;
|
||||
|
||||
} else if (size > 0) {
|
||||
rv = 0;
|
||||
ssize_t remaining = size;
|
||||
ssize_t total_written = 0;
|
||||
ssize_t written = 0;
|
||||
|
||||
do {
|
||||
written = write(dfd, &buffer[total_written], remaining);
|
||||
|
||||
if (written < 0) {
|
||||
warnx("copyFw: couldn't write");
|
||||
rv = -errno;
|
||||
|
||||
} else {
|
||||
total_written += written;
|
||||
remaining -= written;
|
||||
|
||||
@ -166,7 +166,8 @@ private:
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &)> GetSetCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)> ExecuteOpcodeCallback;
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &)>
|
||||
ExecuteOpcodeCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::RestartNode> &)> RestartNodeCallback;
|
||||
void cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::param::GetSet> &result);
|
||||
@ -198,7 +199,8 @@ private:
|
||||
uint8_t _esc_count = 0;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)> EnumerationBeginCallback;
|
||||
void (UavcanServers::*)(const uavcan::ServiceCallResult<uavcan::protocol::enumeration::Begin> &)>
|
||||
EnumerationBeginCallback;
|
||||
typedef uavcan::MethodBinder<UavcanServers *,
|
||||
void (UavcanServers::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::enumeration::Indication>&)>
|
||||
EnumerationIndicationCallback;
|
||||
@ -208,11 +210,12 @@ private:
|
||||
void cb_enumeration_save(const uavcan::ServiceCallResult<uavcan::protocol::param::ExecuteOpcode> &result);
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::indication::BeepCommand> _beep_pub;
|
||||
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback> _enumeration_indication_sub;
|
||||
uavcan::Subscriber<uavcan::protocol::enumeration::Indication, EnumerationIndicationCallback>
|
||||
_enumeration_indication_sub;
|
||||
uavcan::ServiceClient<uavcan::protocol::enumeration::Begin, EnumerationBeginCallback> _enumeration_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::GetSet, GetSetCallback> _enumeration_getset_client;
|
||||
uavcan::ServiceClient<uavcan::protocol::param::ExecuteOpcode, ExecuteOpcodeCallback> _enumeration_save_client;
|
||||
|
||||
void unpackFwFromROMFS(const char* sd_path, const char* romfs_path);
|
||||
int copyFw(const char* dst, const char* src);
|
||||
void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
|
||||
int copyFw(const char *dst, const char *src);
|
||||
};
|
||||
|
||||
@ -116,8 +116,7 @@ public:
|
||||
|
||||
~Queue()
|
||||
{
|
||||
while (!isEmpty())
|
||||
{
|
||||
while (!isEmpty()) {
|
||||
pop();
|
||||
}
|
||||
}
|
||||
@ -140,7 +139,7 @@ public:
|
||||
}
|
||||
|
||||
// Constructing the new item
|
||||
Item *const item = new(ptr) Item(args...);
|
||||
Item *const item = new (ptr) Item(args...);
|
||||
assert(item != nullptr);
|
||||
|
||||
// Inserting the new item at the end of the list
|
||||
@ -353,9 +352,11 @@ class VirtualCanDriver : public uavcan::ICanDriver,
|
||||
int init()
|
||||
{
|
||||
int rv = px4_sem_init(&sem, 0, 0);
|
||||
|
||||
if (rv == 0) {
|
||||
px4_sem_setprotocol(&sem, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
@ -484,9 +485,9 @@ class VirtualCanDriver : public uavcan::ICanDriver,
|
||||
|
||||
public:
|
||||
VirtualCanDriver(unsigned arg_num_ifaces,
|
||||
uavcan::ISystemClock &system_clock,
|
||||
uavcan::IPoolAllocator& allocator,
|
||||
unsigned virtual_iface_block_allocation_quota) :
|
||||
uavcan::ISystemClock &system_clock,
|
||||
uavcan::IPoolAllocator &allocator,
|
||||
unsigned virtual_iface_block_allocation_quota) :
|
||||
num_ifaces_(arg_num_ifaces),
|
||||
clock_(system_clock)
|
||||
{
|
||||
@ -498,8 +499,8 @@ public:
|
||||
const unsigned quota_per_queue = virtual_iface_block_allocation_quota; // 2x overcommit
|
||||
|
||||
for (unsigned i = 0; i < num_ifaces_; i++) {
|
||||
ifaces_[i].construct<uavcan::IPoolAllocator&, uavcan::ISystemClock&, pthread_mutex_t&,
|
||||
unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
|
||||
ifaces_[i].construct<uavcan::IPoolAllocator &, uavcan::ISystemClock &, pthread_mutex_t &,
|
||||
unsigned>(allocator, clock_, driver_mutex_, quota_per_queue);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user