fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14:53:35 +08:00
Code Issues 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);
}
}