fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 00:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed formating and cleanup

Browse Source
This commit is contained in:
blah
2016-03-14 09:07:46 +01:00
committed by Lorenz Meier
parent 147606a399
commit 1188aa138c
4 changed files with 73 additions and 58 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/actuators/hardpoint.cpp
+19 -14
View File
@@ -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
}
src/modules/uavcan/actuators/hardpoint.hpp
+20 -15
View File
@@ -34,7 +34,6 @@
/**
* @file hardpoint.hpp
*
*
* @author Andreas Jochum <Andreas@NicaDrone.com>
*/
@@ -44,8 +43,10 @@
#include <uavcan/equipment/hardpoint/Command.hpp>
#include <uavcan/equipment/hardpoint/Status.hpp>
#include <systemlib/perf_counter.h>
//#include <uORB/topics/esc_status.h>
/**
* @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<UavcanHardpointController *, void (UavcanHardpointController::*)(const uavcan::TimerEvent &)>
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::equipment::hardpoint::Command> _uavcan_pub_raw_cmd;
// uavcan::Subscriber<uavcan::equipment::hardpoint::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _timer;
uavcan::MonotonicTime _prev_cmd_pub; ///< rate limiting
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::hardpoint::Command> _uavcan_pub_raw_cmd;
uavcan::TimerEventForwarder<TimerCbBinder> _timer;
};