From edea4a369ee7d02e4dd74e0fe60e127fe6b57524 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 25 Aug 2017 13:07:21 -0400 Subject: [PATCH] uavcan start enforcing code style (#7856) --- Tools/astyle/files_to_check_code_style.sh | 5 +- src/modules/uavcan/actuators/esc.cpp | 4 +- src/modules/uavcan/allocator.hpp | 15 +- src/modules/uavcan/sensors/gnss.cpp | 173 ++++++++++-------- src/modules/uavcan/sensors/gnss.hpp | 8 +- src/modules/uavcan/sensors/mag.cpp | 3 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 1 + src/modules/uavcan/uavcan_main.cpp | 46 +++-- src/modules/uavcan/uavcan_main.hpp | 2 +- src/modules/uavcan/uavcan_servers.cpp | 150 +++++++++++++-- src/modules/uavcan/uavcan_servers.hpp | 13 +- .../uavcan/uavcan_virtual_can_driver.hpp | 17 +- 12 files changed, 284 insertions(+), 153 deletions(-) diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index b8867200aa..4d03dfd0fa 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -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 diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp index 3a668f5792..8ada8d6b2e 100644 --- a/src/modules/uavcan/actuators/esc.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -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, diff --git a/src/modules/uavcan/allocator.hpp b/src/modules/uavcan/allocator.hpp index 5db3e14940..4d2fe00502 100644 --- a/src/modules/uavcan/allocator.hpp +++ b/src/modules/uavcan/allocator.hpp @@ -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 -{ +struct Allocator : public uavcan::HeapBasedPoolAllocator { static constexpr unsigned CapacitySoftLimit = 250; static constexpr unsigned CapacityHardLimit = 500; @@ -62,11 +60,10 @@ struct Allocator : public uavcan::HeapBasedPoolAllocator 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); + } } }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 8bf4048150..b48ed25e67 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -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 void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure &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 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 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 // 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; diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 2018a71266..0717983d7e 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -80,10 +80,10 @@ private: template void process_fixx(const uavcan::ReceivedDataStructure &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 &); diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 9e7afb95da..fcf344720c 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -142,7 +142,8 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar } } -void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure +void UavcanMagnetometerBridge::mag_sub_cb(const + uavcan::ReceivedDataStructure &msg) { lock(); diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index af4d2cbc0d..fec12209d6 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -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); diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index bbc218ca42..f2f18e921e 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -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 { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 361594ccd1..4c0575e5c0 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -104,7 +104,7 @@ class UavcanNode : public device::CDev public: typedef uavcan_stm32::CanInitHelper CanInitHelper; - enum eServerAction {None, Start, Stop, CheckFW , Busy}; + enum eServerAction {None, Start, Stop, CheckFW, Busy}; UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 02951006fb..208c1d48d7 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -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() = request.real_value; + } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { req.value.to() = request.int_value; + } else { req.value.to() = 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(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(); + } 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(); + } 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(); @@ -642,9 +682,11 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult &msg) +void UavcanServers::cb_enumeration_indication(const + uavcan::ReceivedDataStructure &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() = 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::ServiceCallResultd_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; diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 1fdb69a9b5..acf12c6bbc 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -166,7 +166,8 @@ private: typedef uavcan::MethodBinder &)> GetSetCallback; typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + void (UavcanServers::*)(const uavcan::ServiceCallResult &)> + ExecuteOpcodeCallback; typedef uavcan::MethodBinder &)> RestartNodeCallback; void cb_getset(const uavcan::ServiceCallResult &result); @@ -198,7 +199,8 @@ private: uint8_t _esc_count = 0; typedef uavcan::MethodBinder &)> EnumerationBeginCallback; + void (UavcanServers::*)(const uavcan::ServiceCallResult &)> + EnumerationBeginCallback; typedef uavcan::MethodBinder&)> EnumerationIndicationCallback; @@ -208,11 +210,12 @@ private: void cb_enumeration_save(const uavcan::ServiceCallResult &result); uavcan::Publisher _beep_pub; - uavcan::Subscriber _enumeration_indication_sub; + uavcan::Subscriber + _enumeration_indication_sub; uavcan::ServiceClient _enumeration_client; uavcan::ServiceClient _enumeration_getset_client; uavcan::ServiceClient _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); }; diff --git a/src/modules/uavcan/uavcan_virtual_can_driver.hpp b/src/modules/uavcan/uavcan_virtual_can_driver.hpp index bdd7a4ded8..9ea73533da 100644 --- a/src/modules/uavcan/uavcan_virtual_can_driver.hpp +++ b/src/modules/uavcan/uavcan_virtual_can_driver.hpp @@ -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(allocator, clock_, driver_mutex_, quota_per_queue); + ifaces_[i].construct(allocator, clock_, driver_mutex_, quota_per_queue); } }