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

add ability to control uavcan hardpoint by MAV_CMD_DO_GRIPPER (#19124)

Browse Source 
* update uavcan hardpoint: add ability to use it via mavlink cmd and during mission and removed send_command() dedicated for usage via mavlink console

* uavcan hardpoint: allow gripper ID 0

---------

Co-authored-by: Andrew Brahim <35986980+dirksavage88@users.noreply.github.com>
This commit is contained in:
Dmitry Ponomarev 2026-01-23 00:43:18 +03:00 committed by GitHub
parent 3381b270ea
commit 93ab802202
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 31 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
src/drivers/uavcan/actuators/hardpoint.cpp
View File

@ -38,14 +38,13 @@
*/
#include "hardpoint.hpp"
#include <systemlib/err.h>
UavcanHardpointController::UavcanHardpointController(uavcan::INode &node) :
_node(node),
_uavcan_pub_raw_cmd(node),
_uavcan_pub_hardpoint(node),
_timer(node)
{
_uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::MiddleLower);
_uavcan_pub_hardpoint.setPriority(uavcan::TransferPriority::MiddleLower);
}
UavcanHardpointController::~UavcanHardpointController()
@ -59,33 +58,33 @@ UavcanHardpointController::init()
/*
* Setup timer and call back function for periodic updates
*/
_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
return 0;
}
void
UavcanHardpointController::set_command(uint8_t hardpoint_id, uint16_t command)
{
_cmd.command = command;
_cmd.hardpoint_id = hardpoint_id;
/*
* Publish the command message to the bus
*/
_uavcan_pub_raw_cmd.broadcast(_cmd);
/*
* Start the periodic update timer after a command is set
*/
if (!_timer.isRunning()) {
_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
_timer.setCallback(TimerCbBinder(this, &UavcanHardpointController::periodic_update));
_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_UPDATE_RATE_HZ));
}
return 0;
}
void
UavcanHardpointController::periodic_update(const uavcan::TimerEvent &)
{
// Broadcast command at MAX_RATE_HZ
_uavcan_pub_raw_cmd.broadcast(_cmd);
if (_vehicle_command_sub.updated()) {
vehicle_command_s vehicle_command;
if (_vehicle_command_sub.copy(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
_cmd.hardpoint_id = vehicle_command.param1;
_cmd.command = vehicle_command.param2;
_next_publish_time = 0;
}
}
}
const hrt_abstime timestamp_now = hrt_absolute_time();
if (timestamp_now > _next_publish_time) {
_next_publish_time = timestamp_now + 1000000 / PUBLISH_RATE_HZ;
_uavcan_pub_hardpoint.broadcast(_cmd);
}
}

17
src/drivers/uavcan/actuators/hardpoint.hpp
View File

@ -42,7 +42,8 @@
#include <uavcan/uavcan.hpp>
#include <uavcan/equipment/hardpoint/Command.hpp>
#include <uavcan/equipment/hardpoint/Status.hpp>
#include <perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_command.h>
/**
* @brief The UavcanHardpointController class
@ -59,17 +60,12 @@ public:
*/
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_UPDATE_RATE_HZ = 10;
static constexpr unsigned PUBLISH_RATE_HZ = 1;
uavcan::equipment::hardpoint::Command _cmd;
@ -83,7 +79,10 @@ private:
* libuavcan related things
*/
uavcan::INode &_node;
uavcan::Publisher<uavcan::equipment::hardpoint::Command> _uavcan_pub_raw_cmd;
uavcan::Publisher<uavcan::equipment::hardpoint::Command> _uavcan_pub_hardpoint;
uavcan::TimerEventForwarder<TimerCbBinder> _timer;
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
hrt_abstime _next_publish_time = 0;
};