mirror of https://github.com/ArduPilot/ardupilot
105 lines
3.2 KiB
C++
105 lines
3.2 KiB
C++
/*
|
|
* Copyright (C) 2020 Siddharth B Purohit
|
|
*
|
|
* This file 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 file 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/>.
|
|
*/
|
|
|
|
#include "CANIface.h"
|
|
#include "system.h"
|
|
|
|
bool AP_HAL::CANFrame::priorityHigherThan(const CANFrame& rhs) const
|
|
{
|
|
const uint32_t clean_id = id & MaskExtID;
|
|
const uint32_t rhs_clean_id = rhs.id & MaskExtID;
|
|
|
|
/*
|
|
* STD vs EXT - if 11 most significant bits are the same, EXT loses.
|
|
*/
|
|
const bool ext = id & FlagEFF;
|
|
const bool rhs_ext = rhs.id & FlagEFF;
|
|
if (ext != rhs_ext) {
|
|
const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id;
|
|
const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id;
|
|
if (arb11 != rhs_arb11) {
|
|
return arb11 < rhs_arb11;
|
|
} else {
|
|
return rhs_ext;
|
|
}
|
|
}
|
|
|
|
/*
|
|
* RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
|
|
*/
|
|
const bool rtr = id & FlagRTR;
|
|
const bool rhs_rtr = rhs.id & FlagRTR;
|
|
if (clean_id == rhs_clean_id && rtr != rhs_rtr) {
|
|
return rhs_rtr;
|
|
}
|
|
|
|
/*
|
|
* Plain ID arbitration - greater value loses.
|
|
*/
|
|
return clean_id < rhs_clean_id;
|
|
}
|
|
|
|
/*
|
|
parent class receive handling for MAVCAN
|
|
*/
|
|
int16_t AP_HAL::CANIface::receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags)
|
|
{
|
|
if (frame_callback && (out_flags & IsMAVCAN)==0) {
|
|
frame_callback(get_iface_num(), out_frame);
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
parent class send handling for MAVCAN
|
|
*/
|
|
int16_t AP_HAL::CANIface::send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags)
|
|
{
|
|
if (frame_callback) {
|
|
if ((flags & IsMAVCAN) == 0) {
|
|
if (frame_counter++ == 100) {
|
|
// check every 100 frames for disabling CAN_FRAME send
|
|
// we stop sending after 5s if the client stops
|
|
// sending MAV_CMD_CAN_FORWARD requests
|
|
if (AP_HAL::millis() - last_callback_enable_ms > 5000) {
|
|
frame_callback = nullptr;
|
|
return 1;
|
|
}
|
|
frame_counter = 0;
|
|
}
|
|
frame_callback(get_iface_num(), frame);
|
|
} else {
|
|
CanRxItem rx_item;
|
|
rx_item.frame = frame;
|
|
rx_item.timestamp_us = AP_HAL::native_micros64();
|
|
rx_item.flags = AP_HAL::CANIface::IsMAVCAN;
|
|
add_to_rx_queue(rx_item);
|
|
}
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
/*
|
|
register a callback for for sending CAN_FRAME messages
|
|
*/
|
|
bool AP_HAL::CANIface::register_frame_callback(FrameCb cb)
|
|
{
|
|
last_callback_enable_ms = AP_HAL::millis();
|
|
frame_callback = cb;
|
|
return true;
|
|
}
|