diff --git a/src/modules/uavcan/actuators/hardpoint.cpp b/src/modules/uavcan/actuators/hardpoint.cpp index 3ea985a909..c419790ecb 100644 --- a/src/modules/uavcan/actuators/hardpoint.cpp +++ b/src/modules/uavcan/actuators/hardpoint.cpp @@ -56,6 +56,9 @@ UavcanHardpointController::~UavcanHardpointController() int UavcanHardpointController::init() { + /* + * Setup timer and call back function for periodic updates + */ _timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update)); _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); return 0; @@ -67,32 +70,34 @@ void UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t comma _cmd.command = command; _cmd.hardpoint_id = hardpoint_id; _cmd_set = true; - /* - * Rate limiting - we don't want to congest the bus - */ - const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { - return; - } + /* + * Rate limiting - we don't want to congest the bus + */ + const auto timestamp = _node.getMonotonicTime(); - _prev_cmd_pub = timestamp; + if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + return; + } - /* - * Publish the command message to the bus - */ + _prev_cmd_pub = timestamp; + + /* + * Publish the command message to the bus + */ (void)_uavcan_pub_raw_cmd.broadcast(_cmd); /* * Start the periodic update timer after a command is set */ - if (!_timer.isRunning()) - { + if (!_timer.isRunning()) { _timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ)); } } void UavcanHardpointController::periodic_update(const uavcan::TimerEvent &) { + /* + * Broadcast command at MAX_RATE_HZ + */ (void)_uavcan_pub_raw_cmd.broadcast(_cmd); - //do something } diff --git a/src/modules/uavcan/actuators/hardpoint.hpp b/src/modules/uavcan/actuators/hardpoint.hpp index cd21bf95e0..6b4daa58c8 100644 --- a/src/modules/uavcan/actuators/hardpoint.hpp +++ b/src/modules/uavcan/actuators/hardpoint.hpp @@ -34,7 +34,6 @@ /** * @file hardpoint.hpp * - * * @author Andreas Jochum */ @@ -44,8 +43,10 @@ #include #include #include -//#include +/** + * @brief The UavcanHardpointController class + */ class UavcanHardpointController { @@ -53,34 +54,38 @@ public: UavcanHardpointController(uavcan::INode &node); ~UavcanHardpointController(); + /* + * setup periodic updater + */ int init(); + + /* + * set command + */ void set_command(uint8_t hardpoint_id, uint16_t command); private: + /* + * Max update rate to avoid exessive bus traffic + */ + static constexpr unsigned MAX_RATE_HZ = 1; ///< XXX make this configurable - static constexpr unsigned MAX_RATE_HZ = 1; ///< XXX make this configurable + uavcan::equipment::hardpoint::Command _cmd; - - uavcan::equipment::hardpoint::Command _cmd; - - bool _cmd_set = false; + bool _cmd_set = false; void periodic_update(const uavcan::TimerEvent &); typedef uavcan::MethodBinder TimerCbBinder; - // hardpoint_status_s _hardpoint_status = {}; - orb_advert_t _hardpoint_status_pub = nullptr; - /* * libuavcan related things */ - uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting - uavcan::INode &_node; - uavcan::Publisher _uavcan_pub_raw_cmd; - // uavcan::Subscriber _uavcan_sub_status; - uavcan::TimerEventForwarder _timer; + uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting + uavcan::INode &_node; + uavcan::Publisher _uavcan_pub_raw_cmd; + uavcan::TimerEventForwarder _timer; }; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index dd7bcdc9d3..22a6c817b1 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -31,6 +31,16 @@ * ****************************************************************************/ +/** + * @file uavcan_main.cpp + * + * Implements basic functionality of UAVCAN node. + * + * @author Pavel Kirienko + * David Sidrane + * Andreas Jochum + */ + #include #include @@ -62,16 +72,6 @@ // #include and leaving OK undefined # define OK 0 - -/** - * @file uavcan_main.cpp - * - * Implements basic functionality of UAVCAN node. - * - * @author Pavel Kirienko - * David Sidrane - */ - /* * UavcanNode */ @@ -1110,8 +1110,8 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg) case UAVCANIOC_HARDPOINT_SET: { - const auto& cmd = *reinterpret_cast(arg); - _hardpoint_controller.set_command(cmd.hardpoint_id, cmd.command); + const auto &cmd = *reinterpret_cast(arg); + _hardpoint_controller.set_command(cmd.hardpoint_id, cmd.command); } break; @@ -1141,7 +1141,7 @@ UavcanNode::print_info() // Memory status printf("Pool allocator status:\n"); printf("\tCapacity hard/soft: %u/%u blocks\n", - _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); + _pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity()); printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks()); printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks()); @@ -1222,7 +1222,8 @@ void UavcanNode::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) +{ _hardpoint_controller.set_command(hardpoint_id, command); } /* @@ -1375,6 +1376,7 @@ int uavcan_main(int argc, char *argv[]) return inst->set_param(nodeid, argv[4], argv[5]); } } + if (!std::strcmp(argv[1], "hardpoint")) { if (!std::strcmp(argv[2], "set") && argc > 4) { const int hardpoint_id = atoi(argv[3]); @@ -1382,19 +1384,20 @@ int uavcan_main(int argc, char *argv[]) // Sanity check - weed out negative values, check against maximums if (hardpoint_id >= 0 && - hardpoint_id < 256 && - command >= 0 && - command < 65536) { + hardpoint_id < 256 && + command >= 0 && + command < 65536) { inst->hardpoint_controller_set((uint8_t) hardpoint_id, (uint16_t) command); - } - else { + + } else { errx(1, "Are you nuts?"); } - } - else { + + } else { errx(1, "Are you nuts?"); } } + if (!std::strcmp(argv[1], "stop")) { if (fw) { diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 45ed9dedeb..02a4c4bb70 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -31,6 +31,15 @@ * ****************************************************************************/ +/** + * @file uavcan_main.hpp + * + * Defines basic functinality of UAVCAN node. + * + * @author Pavel Kirienko + * Andreas Jochum + */ + #pragma once #include @@ -59,14 +68,6 @@ #include "uavcan_servers.hpp" #include "allocator.hpp" -/** - * @file uavcan_main.hpp - * - * Defines basic functinality of UAVCAN node. - * - * @author Pavel Kirienko - */ - #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 4 // we add two to allow for actuator_direct and busevent @@ -124,6 +125,7 @@ public: void subscribe(); int teardown(); + int arm_actuators(bool arm); void print_info();