mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 15:08:28 -04:00
a6246a6afa
AP_Arming tacks on the sub-system bit. Remove PiccoloCAN's silly nullptr check Require the library to supply the failure message (no default message) Remove default cases so authors know to think about places they should add things.
752 lines
29 KiB
C++
752 lines
29 KiB
C++
/*
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
/*
|
|
* AP_KDECAN.cpp
|
|
*
|
|
* Author: Francisco Ferreira
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#if HAL_WITH_UAVCAN
|
|
|
|
#include <AP_BoardConfig/AP_BoardConfig.h>
|
|
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
|
|
|
|
#include <AP_Common/AP_Common.h>
|
|
|
|
#include <AP_HAL/utility/sparse-endian.h>
|
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
|
#include <GCS_MAVLink/GCS.h>
|
|
#include <AP_Scheduler/AP_Scheduler.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Motors/AP_Motors.h>
|
|
#include <AP_Logger/AP_Logger.h>
|
|
|
|
#include "AP_KDECAN.h"
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define debug_can(level_debug, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(_driver_index)) { printf(fmt, ##args); }} while (0)
|
|
|
|
#define DEFAULT_NUM_POLES 14
|
|
|
|
// table of user settable CAN bus parameters
|
|
const AP_Param::GroupInfo AP_KDECAN::var_info[] = {
|
|
// @Param: NPOLE
|
|
// @DisplayName: Number of motor poles
|
|
// @Description: Sets the number of motor poles to calculate the correct RPM value
|
|
AP_GROUPINFO("NPOLE", 1, AP_KDECAN, _num_poles, DEFAULT_NUM_POLES),
|
|
|
|
AP_GROUPEND
|
|
};
|
|
|
|
const uint16_t AP_KDECAN::SET_PWM_MIN_INTERVAL_US;
|
|
|
|
AP_KDECAN::AP_KDECAN()
|
|
{
|
|
AP_Param::setup_object_defaults(this, var_info);
|
|
|
|
debug_can(2, "KDECAN: constructed\n\r");
|
|
}
|
|
|
|
AP_KDECAN *AP_KDECAN::get_kdecan(uint8_t driver_index)
|
|
{
|
|
if (driver_index >= AP::can().get_num_drivers() ||
|
|
AP::can().get_protocol_type(driver_index) != AP_BoardConfig_CAN::Protocol_Type_KDECAN) {
|
|
return nullptr;
|
|
}
|
|
return static_cast<AP_KDECAN*>(AP::can().get_driver(driver_index));
|
|
}
|
|
|
|
void AP_KDECAN::init(uint8_t driver_index, bool enable_filters)
|
|
{
|
|
_driver_index = driver_index;
|
|
|
|
debug_can(2, "KDECAN: starting init\n\r");
|
|
|
|
if (_initialized) {
|
|
debug_can(1, "KDECAN: already initialized\n\r");
|
|
return;
|
|
}
|
|
|
|
// get CAN manager instance
|
|
AP_HAL::CANManager* can_mgr = hal.can_mgr[driver_index];
|
|
|
|
if (can_mgr == nullptr) {
|
|
debug_can(1, "KDECAN: no mgr for this driver\n\r");
|
|
return;
|
|
}
|
|
|
|
if (!can_mgr->is_initialized()) {
|
|
debug_can(1, "KDECAN: mgr not initialized\n\r");
|
|
return;
|
|
}
|
|
|
|
// store pointer to CAN driver
|
|
_can_driver = can_mgr->get_driver();
|
|
|
|
if (_can_driver == nullptr) {
|
|
debug_can(1, "KDECAN: no CAN driver\n\r");
|
|
return;
|
|
}
|
|
|
|
// find available KDE ESCs
|
|
frame_id_t id = { { .object_address = ESC_INFO_OBJ_ADDR,
|
|
.destination_id = BROADCAST_NODE_ID,
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
|
|
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 };
|
|
|
|
if(!_can_driver->getIface(CAN_IFACE_INDEX)->send(frame, uavcan::MonotonicTime::fromMSec(AP_HAL::millis() + 1000), 0)) {
|
|
debug_can(1, "KDECAN: couldn't send discovery message\n\r");
|
|
return;
|
|
}
|
|
|
|
debug_can(2, "KDECAN: discovery message sent\n\r");
|
|
|
|
uint32_t start = AP_HAL::millis();
|
|
|
|
// wait 1 second for answers
|
|
while (AP_HAL::millis() - start < 1000) {
|
|
uavcan::CanFrame esc_id_frame {};
|
|
uavcan::MonotonicTime time {};
|
|
uavcan::UtcTime utc_time {};
|
|
uavcan::CanIOFlags flags {};
|
|
|
|
int16_t n = _can_driver->getIface(CAN_IFACE_INDEX)->receive(esc_id_frame, time, utc_time, flags);
|
|
|
|
if (n != 1) {
|
|
continue;
|
|
}
|
|
|
|
if (!esc_id_frame.isExtended()) {
|
|
continue;
|
|
}
|
|
|
|
if (esc_id_frame.dlc != 5) {
|
|
continue;
|
|
}
|
|
|
|
id.value = esc_id_frame.id & uavcan::CanFrame::MaskExtID;
|
|
|
|
if (id.source_id == BROADCAST_NODE_ID ||
|
|
id.source_id >= (KDECAN_MAX_NUM_ESCS + ESC_NODE_ID_FIRST) ||
|
|
id.destination_id != AUTOPILOT_NODE_ID ||
|
|
id.object_address != ESC_INFO_OBJ_ADDR) {
|
|
continue;
|
|
}
|
|
|
|
_esc_present_bitmask |= (1 << (id.source_id - ESC_NODE_ID_FIRST));
|
|
_esc_max_node_id = id.source_id - ESC_NODE_ID_FIRST + 1;
|
|
|
|
debug_can(2, "KDECAN: found ESC id %u\n\r", id.source_id);
|
|
}
|
|
|
|
snprintf(_thread_name, sizeof(_thread_name), "kdecan_%u", driver_index);
|
|
|
|
// start thread for receiving and sending CAN frames
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_KDECAN::loop, void), _thread_name, 4096, AP_HAL::Scheduler::PRIORITY_CAN, 0)) {
|
|
debug_can(1, "KDECAN: couldn't create thread\n\r");
|
|
return;
|
|
}
|
|
|
|
_initialized = true;
|
|
|
|
debug_can(2, "KDECAN: init done\n\r");
|
|
|
|
return;
|
|
}
|
|
|
|
void AP_KDECAN::loop()
|
|
{
|
|
uavcan::MonotonicTime timeout;
|
|
uavcan::CanFrame empty_frame { (0 | uavcan::CanFrame::FlagEFF), nullptr, 0 };
|
|
const uavcan::CanFrame* select_frames[uavcan::MaxCanIfaces] { };
|
|
select_frames[CAN_IFACE_INDEX] = &empty_frame;
|
|
|
|
uint16_t output_buffer[KDECAN_MAX_NUM_ESCS] {};
|
|
|
|
enumeration_state_t enumeration_state = _enumeration_state;
|
|
uint64_t enumeration_start = 0;
|
|
uint8_t enumeration_esc_num = 0;
|
|
|
|
const uint32_t LOOP_INTERVAL_US = MIN(AP::scheduler().get_loop_period_us(), SET_PWM_MIN_INTERVAL_US);
|
|
uint64_t pwm_last_sent = 0;
|
|
uint8_t sending_esc_num = 0;
|
|
|
|
uint64_t telemetry_last_request = 0;
|
|
|
|
while (true) {
|
|
if (!_initialized) {
|
|
debug_can(2, "KDECAN: not initialized\n\r");
|
|
hal.scheduler->delay_microseconds(2000);
|
|
continue;
|
|
}
|
|
|
|
uavcan::CanSelectMasks inout_mask;
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
// get latest enumeration state set from GCS
|
|
if (_enum_sem.take(1)) {
|
|
enumeration_state = _enumeration_state;
|
|
_enum_sem.give();
|
|
} else {
|
|
debug_can(2, "KDECAN: failed to get enumeration semaphore on loop\n\r");
|
|
}
|
|
|
|
if (enumeration_state != ENUMERATION_STOPPED) {
|
|
// check if enumeration timed out
|
|
if (enumeration_start != 0 && now - enumeration_start >= ENUMERATION_TIMEOUT_MS * 1000) {
|
|
enumeration_start = 0;
|
|
|
|
WITH_SEMAPHORE(_enum_sem);
|
|
|
|
// check if enumeration state didn't change or was set to stop
|
|
if (enumeration_state == _enumeration_state || _enumeration_state == ENUMERATION_STOP) {
|
|
enumeration_state = _enumeration_state = ENUMERATION_STOPPED;
|
|
}
|
|
|
|
continue;
|
|
}
|
|
|
|
timeout = uavcan::MonotonicTime::fromUSec(now + 1000);
|
|
|
|
switch (enumeration_state) {
|
|
case ENUMERATION_START: {
|
|
inout_mask.write = 1 << CAN_IFACE_INDEX;
|
|
|
|
// send broadcast frame to start enumeration
|
|
frame_id_t id = { { .object_address = ENUM_OBJ_ADDR,
|
|
.destination_id = BROADCAST_NODE_ID,
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
be16_t data = htobe16((uint16_t) ENUMERATION_TIMEOUT_MS);
|
|
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) };
|
|
|
|
uavcan::CanSelectMasks in_mask = inout_mask;
|
|
select_frames[CAN_IFACE_INDEX] = &frame;
|
|
|
|
// wait for write space to be available
|
|
_can_driver->select(inout_mask, select_frames, timeout);
|
|
select_frames[CAN_IFACE_INDEX] = &empty_frame;
|
|
|
|
if (in_mask.write & inout_mask.write) {
|
|
now = AP_HAL::micros64();
|
|
timeout = uavcan::MonotonicTime::fromUSec(now + ENUMERATION_TIMEOUT_MS * 1000);
|
|
|
|
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
|
|
|
|
if (res == 1) {
|
|
enumeration_start = now;
|
|
enumeration_esc_num = 0;
|
|
_esc_present_bitmask = 0;
|
|
_esc_max_node_id = 0;
|
|
|
|
WITH_SEMAPHORE(_enum_sem);
|
|
|
|
if (enumeration_state == _enumeration_state) {
|
|
enumeration_state = _enumeration_state = ENUMERATION_RUNNING;
|
|
}
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange buffer full when starting ESC enumeration\n\r");
|
|
break;
|
|
} else {
|
|
debug_can(1, "KDECAN: error sending message to start ESC enumeration, result %d\n\r", res);
|
|
break;
|
|
}
|
|
} else {
|
|
break;
|
|
}
|
|
FALLTHROUGH;
|
|
}
|
|
case ENUMERATION_RUNNING: {
|
|
inout_mask.read = 1 << CAN_IFACE_INDEX;
|
|
inout_mask.write = 0;
|
|
|
|
// wait for enumeration messages from ESCs
|
|
uavcan::CanSelectMasks in_mask = inout_mask;
|
|
_can_driver->select(inout_mask, select_frames, timeout);
|
|
|
|
if (in_mask.read & inout_mask.read) {
|
|
uavcan::CanFrame recv_frame;
|
|
uavcan::MonotonicTime time;
|
|
uavcan::UtcTime utc_time;
|
|
uavcan::CanIOFlags flags {};
|
|
|
|
int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(recv_frame, time, utc_time, flags);
|
|
|
|
if (res == 1) {
|
|
if (time.toUSec() < enumeration_start) {
|
|
// old message
|
|
break;
|
|
}
|
|
|
|
frame_id_t id { .value = recv_frame.id & uavcan::CanFrame::MaskExtID };
|
|
|
|
if (id.object_address == UPDATE_NODE_ID_OBJ_ADDR) {
|
|
// reply from setting new node ID
|
|
_esc_present_bitmask |= 1 << (id.source_id - ESC_NODE_ID_FIRST);
|
|
_esc_max_node_id = MAX(_esc_max_node_id, id.source_id - ESC_NODE_ID_FIRST + 1);
|
|
break;
|
|
} else if (id.object_address != ENUM_OBJ_ADDR) {
|
|
// discardable frame, only looking for enumeration
|
|
break;
|
|
}
|
|
|
|
// try to set node ID for the received ESC
|
|
while (AP_HAL::micros64() - enumeration_start < ENUMERATION_TIMEOUT_MS * 1000) {
|
|
inout_mask.read = 0;
|
|
inout_mask.write = 1 << CAN_IFACE_INDEX;
|
|
|
|
// wait for write space to be available
|
|
in_mask = inout_mask;
|
|
_can_driver->select(inout_mask, select_frames, timeout);
|
|
|
|
if (in_mask.write & inout_mask.write) {
|
|
id = { { .object_address = UPDATE_NODE_ID_OBJ_ADDR,
|
|
.destination_id = uint8_t(enumeration_esc_num + ESC_NODE_ID_FIRST),
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
uavcan::CanFrame send_frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &recv_frame.data, recv_frame.dlc };
|
|
timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000);
|
|
|
|
res = _can_driver->getIface(CAN_IFACE_INDEX)->send(send_frame, timeout, 0);
|
|
|
|
if (res == 1) {
|
|
enumeration_esc_num++;
|
|
break;
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange buffer full when setting ESC node ID\n\r");
|
|
} else {
|
|
debug_can(1, "KDECAN: error sending message to set ESC node ID, result %d\n\r", res);
|
|
}
|
|
}
|
|
}
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange failed read when getting ESC enumeration message\n\r");
|
|
} else {
|
|
debug_can(1, "KDECAN: error receiving ESC enumeration message, result %d\n\r", res);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case ENUMERATION_STOP: {
|
|
inout_mask.write = 1 << CAN_IFACE_INDEX;
|
|
|
|
// send broadcast frame to stop enumeration
|
|
frame_id_t id = { { .object_address = ENUM_OBJ_ADDR,
|
|
.destination_id = BROADCAST_NODE_ID,
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
le16_t data = htole16((uint16_t) ENUMERATION_TIMEOUT_MS);
|
|
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &data, sizeof(data) };
|
|
|
|
uavcan::CanSelectMasks in_mask = inout_mask;
|
|
select_frames[CAN_IFACE_INDEX] = &frame;
|
|
|
|
// wait for write space to be available
|
|
_can_driver->select(inout_mask, select_frames, timeout);
|
|
select_frames[CAN_IFACE_INDEX] = &empty_frame;
|
|
|
|
if (in_mask.write & inout_mask.write) {
|
|
timeout = uavcan::MonotonicTime::fromUSec(enumeration_start + ENUMERATION_TIMEOUT_MS * 1000);
|
|
|
|
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
|
|
|
|
if (res == 1) {
|
|
enumeration_start = 0;
|
|
|
|
WITH_SEMAPHORE(_enum_sem);
|
|
|
|
if (enumeration_state == _enumeration_state) {
|
|
enumeration_state = _enumeration_state = ENUMERATION_STOPPED;
|
|
}
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange buffer full when stop ESC enumeration\n\r");
|
|
} else {
|
|
debug_can(1, "KDECAN: error sending message to stop ESC enumeration, result %d\n\r", res);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case ENUMERATION_STOPPED:
|
|
default:
|
|
debug_can(2, "KDECAN: something wrong happened, shouldn't be here, enumeration state: %u\n\r", enumeration_state);
|
|
break;
|
|
}
|
|
|
|
continue;
|
|
}
|
|
|
|
if (!_esc_present_bitmask) {
|
|
debug_can(1, "KDECAN: no valid ESC present");
|
|
hal.scheduler->delay(1000);
|
|
continue;
|
|
}
|
|
|
|
// always look for received frames
|
|
inout_mask.read = 1 << CAN_IFACE_INDEX;
|
|
timeout = uavcan::MonotonicTime::fromUSec(now + LOOP_INTERVAL_US);
|
|
|
|
// check if:
|
|
// - is currently sending throttle frames, OR
|
|
// - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR
|
|
// - it is time to send throttle frames again, regardless of new output values, OR
|
|
// - it is time to ask for telemetry information
|
|
if (sending_esc_num > 0 ||
|
|
(_new_output.load(std::memory_order_acquire) && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) ||
|
|
(pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US)) ||
|
|
(now - telemetry_last_request > TELEMETRY_INTERVAL_US)) {
|
|
|
|
inout_mask.write = 1 << CAN_IFACE_INDEX;
|
|
} else { // don't need to send frame, choose the maximum time we'll wait for receiving a frame
|
|
uint64_t next_action = MIN(now + LOOP_INTERVAL_US, telemetry_last_request + TELEMETRY_INTERVAL_US);
|
|
|
|
if (pwm_last_sent != 0) {
|
|
next_action = MIN(next_action, pwm_last_sent + SET_PWM_MIN_INTERVAL_US);
|
|
}
|
|
|
|
timeout = uavcan::MonotonicTime::fromUSec(next_action);
|
|
}
|
|
|
|
// wait for write space or receive frame
|
|
uavcan::CanSelectMasks in_mask = inout_mask;
|
|
_can_driver->select(inout_mask, select_frames, timeout);
|
|
|
|
if (in_mask.read & inout_mask.read) {
|
|
uavcan::CanFrame frame;
|
|
uavcan::MonotonicTime time;
|
|
uavcan::UtcTime utc_time;
|
|
uavcan::CanIOFlags flags {};
|
|
|
|
int16_t res = _can_driver->getIface(CAN_IFACE_INDEX)->receive(frame, time, utc_time, flags);
|
|
|
|
if (res == 1) {
|
|
frame_id_t id { .value = frame.id & uavcan::CanFrame::MaskExtID };
|
|
|
|
// check if frame is valid: directed at autopilot, doesn't come from broadcast and ESC was detected before
|
|
if (id.destination_id == AUTOPILOT_NODE_ID &&
|
|
id.source_id != BROADCAST_NODE_ID &&
|
|
(1 << (id.source_id - ESC_NODE_ID_FIRST) & _esc_present_bitmask)) {
|
|
switch (id.object_address) {
|
|
case TELEMETRY_OBJ_ADDR: {
|
|
if (frame.dlc != 8) {
|
|
break;
|
|
}
|
|
|
|
if (!_telem_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get telemetry semaphore on write\n\r");
|
|
break;
|
|
}
|
|
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].time = time.toUSec();
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].voltage = frame.data[0] << 8 | frame.data[1];
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].current = frame.data[2] << 8 | frame.data[3];
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].rpm = frame.data[4] << 8 | frame.data[5];
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].temp = frame.data[6];
|
|
_telemetry[id.source_id - ESC_NODE_ID_FIRST].new_data = true;
|
|
_telem_sem.give();
|
|
break;
|
|
}
|
|
default:
|
|
// discard frame
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (in_mask.write & inout_mask.write) {
|
|
now = AP_HAL::micros64();
|
|
|
|
bool new_output = _new_output.load(std::memory_order_acquire);
|
|
|
|
if (sending_esc_num > 0) {
|
|
// currently sending throttle frames, check it didn't timeout
|
|
if (now - pwm_last_sent > SET_PWM_TIMEOUT_US) {
|
|
debug_can(2, "KDECAN: timed-out after sending frame to ESC with ID %d\n\r", sending_esc_num - 1);
|
|
sending_esc_num = 0;
|
|
}
|
|
}
|
|
|
|
if (sending_esc_num == 0 && new_output) {
|
|
if (!_rc_out_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get PWM semaphore on read\n\r");
|
|
continue;
|
|
}
|
|
|
|
memcpy(output_buffer, _scaled_output, KDECAN_MAX_NUM_ESCS * sizeof(uint16_t));
|
|
|
|
_rc_out_sem.give();
|
|
}
|
|
|
|
// check if:
|
|
// - is currently sending throttle frames, OR
|
|
// - there are new output values and, a throttle frame was never sent or it's no longer in CAN queue, OR
|
|
// - it is time to send throttle frames again, regardless of new output values
|
|
if (sending_esc_num > 0 ||
|
|
(new_output && (pwm_last_sent == 0 || now - pwm_last_sent > SET_PWM_TIMEOUT_US)) ||
|
|
(pwm_last_sent != 0 && (now - pwm_last_sent > SET_PWM_MIN_INTERVAL_US))) {
|
|
|
|
for (uint8_t esc_num = sending_esc_num; esc_num < _esc_max_node_id; esc_num++) {
|
|
|
|
if ((_esc_present_bitmask & (1 << esc_num)) == 0) {
|
|
continue;
|
|
}
|
|
|
|
be16_t kde_pwm = htobe16(output_buffer[esc_num]);
|
|
|
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
|
kde_pwm = 0;
|
|
}
|
|
|
|
frame_id_t id = { { .object_address = SET_PWM_OBJ_ADDR,
|
|
.destination_id = uint8_t(esc_num + ESC_NODE_ID_FIRST),
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
|
|
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), (uint8_t*) &kde_pwm, sizeof(kde_pwm) };
|
|
|
|
if (esc_num == 0) {
|
|
timeout = uavcan::MonotonicTime::fromUSec(now + SET_PWM_TIMEOUT_US);
|
|
} else {
|
|
timeout = uavcan::MonotonicTime::fromUSec(pwm_last_sent + SET_PWM_TIMEOUT_US);
|
|
}
|
|
|
|
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
|
|
|
|
if (res == 1) {
|
|
if (esc_num == 0) {
|
|
pwm_last_sent = now;
|
|
|
|
if (new_output) {
|
|
_new_output.store(false, std::memory_order_release);
|
|
}
|
|
}
|
|
|
|
sending_esc_num = (esc_num + 1) % _esc_max_node_id;
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange buffer full when sending message to ESC with ID %d\n\r", esc_num + ESC_NODE_ID_FIRST);
|
|
} else {
|
|
debug_can(1, "KDECAN: error sending message to ESC with ID %d, result %d\n\r", esc_num + ESC_NODE_ID_FIRST, res);
|
|
}
|
|
|
|
break;
|
|
}
|
|
} else if (now - telemetry_last_request > TELEMETRY_INTERVAL_US) {
|
|
// broadcast telemetry request frame
|
|
frame_id_t id = { { .object_address = TELEMETRY_OBJ_ADDR,
|
|
.destination_id = BROADCAST_NODE_ID,
|
|
.source_id = AUTOPILOT_NODE_ID,
|
|
.priority = 0,
|
|
.unused = 0 } };
|
|
|
|
uavcan::CanFrame frame { (id.value | uavcan::CanFrame::FlagEFF), nullptr, 0 };
|
|
timeout = uavcan::MonotonicTime::fromUSec(now + TELEMETRY_TIMEOUT_US);
|
|
|
|
int8_t res = _can_driver->getIface(CAN_IFACE_INDEX)->send(frame, timeout, 0);
|
|
|
|
if (res == 1) {
|
|
telemetry_last_request = now;
|
|
} else if (res == 0) {
|
|
debug_can(1, "KDECAN: strange buffer full when sending message requesting telemetry\n\r");
|
|
} else {
|
|
debug_can(1, "KDECAN: error sending message requesting telemetry, result %d\n\r", res);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void AP_KDECAN::update()
|
|
{
|
|
if (_rc_out_sem.take(1)) {
|
|
for (uint8_t i = 0; i < KDECAN_MAX_NUM_ESCS; i++) {
|
|
if ((_esc_present_bitmask & (1 << i)) == 0) {
|
|
continue;
|
|
}
|
|
|
|
SRV_Channel::Aux_servo_function_t motor_function = SRV_Channels::get_motor_function(i);
|
|
|
|
if (SRV_Channels::function_assigned(motor_function)) {
|
|
float norm_output = SRV_Channels::get_output_norm(motor_function);
|
|
_scaled_output[i] = uint16_t((norm_output + 1.0f) / 2.0f * 2000.0f);
|
|
} else {
|
|
_scaled_output[i] = 0;
|
|
}
|
|
}
|
|
|
|
_rc_out_sem.give();
|
|
_new_output.store(true, std::memory_order_release);
|
|
} else {
|
|
debug_can(2, "KDECAN: failed to get PWM semaphore on write\n\r");
|
|
}
|
|
|
|
AP_Logger *logger = AP_Logger::get_singleton();
|
|
|
|
if (logger == nullptr || !logger->should_log(0xFFFFFFFF)) {
|
|
return;
|
|
}
|
|
|
|
if (!_telem_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get telemetry semaphore on DF read\n\r");
|
|
return;
|
|
}
|
|
|
|
telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS] {};
|
|
|
|
for (uint8_t i = 0; i < _esc_max_node_id; i++) {
|
|
if (_telemetry[i].new_data) {
|
|
telem_buffer[i] = _telemetry[i];
|
|
_telemetry[i].new_data = false;
|
|
}
|
|
}
|
|
|
|
_telem_sem.give();
|
|
|
|
uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES;
|
|
|
|
// log ESC telemetry data
|
|
for (uint8_t i = 0; i < _esc_max_node_id; i++) {
|
|
if (telem_buffer[i].new_data) {
|
|
logger->Write_ESC(i, telem_buffer[i].time,
|
|
int32_t(telem_buffer[i].rpm * 60UL * 2 / num_poles * 100),
|
|
telem_buffer[i].voltage,
|
|
telem_buffer[i].current,
|
|
int16_t(telem_buffer[i].temp * 100U), 0, 0);
|
|
}
|
|
}
|
|
}
|
|
|
|
bool AP_KDECAN::pre_arm_check(char* reason, uint8_t reason_len)
|
|
{
|
|
if (!_enum_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get enumeration semaphore on read\n\r");
|
|
snprintf(reason, reason_len ,"Enumeration state unknown");
|
|
return false;
|
|
}
|
|
|
|
if (_enumeration_state != ENUMERATION_STOPPED) {
|
|
snprintf(reason, reason_len ,"Enumeration running");
|
|
_enum_sem.give();
|
|
return false;
|
|
}
|
|
|
|
_enum_sem.give();
|
|
|
|
uint16_t motors_mask = 0;
|
|
AP_Motors *motors = AP_Motors::get_singleton();
|
|
|
|
if (motors) {
|
|
motors_mask = motors->get_motor_mask();
|
|
}
|
|
|
|
uint8_t num_expected_motors = __builtin_popcount(motors_mask);
|
|
uint8_t num_present_escs = __builtin_popcount(_esc_present_bitmask);
|
|
|
|
if (num_present_escs < num_expected_motors) {
|
|
snprintf(reason, reason_len, "Too few ESCs detected");
|
|
return false;
|
|
}
|
|
|
|
if (num_present_escs > num_expected_motors) {
|
|
snprintf(reason, reason_len, "Too many ESCs detected");
|
|
return false;
|
|
}
|
|
|
|
if (_esc_max_node_id != num_expected_motors) {
|
|
snprintf(reason, reason_len, "Wrong node IDs, run enumeration");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void AP_KDECAN::send_mavlink(uint8_t chan)
|
|
{
|
|
if (!_telem_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get telemetry semaphore on MAVLink read\n\r");
|
|
return;
|
|
}
|
|
|
|
telemetry_info_t telem_buffer[KDECAN_MAX_NUM_ESCS];
|
|
memcpy(telem_buffer, _telemetry, sizeof(telemetry_info_t) * KDECAN_MAX_NUM_ESCS);
|
|
_telem_sem.give();
|
|
|
|
uint16_t voltage[4] {};
|
|
uint16_t current[4] {};
|
|
uint16_t rpm[4] {};
|
|
uint8_t temperature[4] {};
|
|
uint16_t totalcurrent[4] {};
|
|
uint16_t count[4] {};
|
|
uint8_t num_poles = _num_poles > 0 ? _num_poles : DEFAULT_NUM_POLES;
|
|
uint64_t now = AP_HAL::micros64();
|
|
|
|
for (uint8_t i = 0; i < _esc_max_node_id && i < 8; i++) {
|
|
uint8_t idx = i % 4;
|
|
if (telem_buffer[i].time && (now - telem_buffer[i].time < 1000000)) {
|
|
voltage[idx] = telem_buffer[i].voltage;
|
|
current[idx] = telem_buffer[i].current;
|
|
rpm[idx] = uint16_t(telem_buffer[i].rpm * 60UL * 2 / num_poles);
|
|
temperature[idx] = telem_buffer[i].temp;
|
|
} else {
|
|
voltage[idx] = 0;
|
|
current[idx] = 0;
|
|
rpm[idx] = 0;
|
|
temperature[idx] = 0;
|
|
}
|
|
|
|
if (idx == 3 || i == _esc_max_node_id - 1) {
|
|
if (!HAVE_PAYLOAD_SPACE((mavlink_channel_t)chan, ESC_TELEMETRY_1_TO_4)) {
|
|
return;
|
|
}
|
|
|
|
if (i < 4) {
|
|
mavlink_msg_esc_telemetry_1_to_4_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count);
|
|
} else {
|
|
mavlink_msg_esc_telemetry_5_to_8_send((mavlink_channel_t)chan, temperature, voltage, current, totalcurrent, rpm, count);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
bool AP_KDECAN::run_enumeration(bool start_stop)
|
|
{
|
|
if (!_enum_sem.take(1)) {
|
|
debug_can(2, "KDECAN: failed to get enumeration semaphore on write\n\r");
|
|
return false;
|
|
}
|
|
|
|
if (start_stop) {
|
|
_enumeration_state = ENUMERATION_START;
|
|
} else if (_enumeration_state != ENUMERATION_STOPPED) {
|
|
_enumeration_state = ENUMERATION_STOP;
|
|
}
|
|
|
|
_enum_sem.give();
|
|
|
|
return true;
|
|
}
|
|
|
|
#endif // HAL_WITH_UAVCAN
|