Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 14:47:44 +08:00
Code Issues Actions 16 Packages Projects Releases Wiki Activity

ESC status feedback

Browse Source
This commit is contained in:
Pavel Kirienko
2014-10-12 20:56:45 +04:00
parent ced75deebe
commit e5a77a638a
2 changed files with 33 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uavcan/actuators/esc.cpp
+26 -3
View File
@@ -73,7 +73,7 @@ int UavcanEscController::init()
void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
{
if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
if ((outputs == nullptr) || (num_outputs > uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize)) {
perf_count(_perfcnt_invalid_input);
return;
}
@@ -126,10 +126,33 @@ void UavcanEscController::arm_esc(bool arm)
void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
{
// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
if (msg.esc_index < CONNECTED_ESC_MAX) {
_esc_status.esc_count = uavcan::max<int>(_esc_status.esc_count, msg.esc_index + 1);
_esc_status.timestamp = msg.getMonotonicTimestamp().toUSec();
auto &ref = _esc_status.esc[msg.esc_index];
ref.esc_address = msg.getSrcNodeID().get();
// >0 checks allow to weed out NaNs and negative values that aren't supported.
ref.esc_voltage = (msg.voltage > 0) ? msg.voltage * 10.0F : 0;
ref.esc_current = (msg.current > 0) ? msg.current * 10.0F : 0;
ref.esc_temperature = (msg.temperature > 0) ? msg.temperature * 10.0F : 0;
ref.esc_setpoint = msg.power_rating_pct;
ref.esc_rpm = abs(msg.rpm);
ref.esc_errorcount = msg.error_count;
}
}
void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
{
// TODO publish to ORB
_esc_status.counter += 1;
_esc_status.esc_connectiontype = ESC_CONNECTION_TYPE_CAN;
if (_esc_status_pub > 0) {
(void)orb_publish(ORB_ID(esc_status), _esc_status_pub, &_esc_status);
} else {
_esc_status_pub = orb_advertise(ORB_ID(esc_status), &_esc_status);
}
}
src/modules/uavcan/actuators/esc.hpp
+7 -8
View File
@@ -48,6 +48,8 @@
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <systemlib/perf_counter.h>
#include <uORB/topics/esc_status.h>
class UavcanEscController
{
@@ -74,8 +76,7 @@ private:
static constexpr unsigned MAX_RATE_HZ = 200; ///< XXX make this configurable
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 10;
typedef uavcan::MethodBinder<UavcanEscController*,
void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
@@ -84,6 +85,10 @@ private:
typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
TimerCbBinder;
bool _armed = false;
esc_status_s _esc_status = {};
orb_advert_t _esc_status_pub = -1;
/*
* libuavcan related things
*/
@@ -93,12 +98,6 @@ private:
uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder> _uavcan_sub_status;
uavcan::TimerEventForwarder<TimerCbBinder> _orb_timer;
/*
* ESC states
*/
bool _armed = false;
uavcan::equipment::esc::Status _states[MAX_ESCS];
/*
* Perf counters
*/