ardupilot/libraries/AP_HAL/CANIface.cpp

96 lines
2.7 KiB
C++
Raw Normal View History

/*
* 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)
{
auto cb = frame_callback;
if (cb && (out_flags & IsMAVCAN)==0) {
cb(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)
{
auto cb = frame_callback;
if (cb) {
if ((flags & IsMAVCAN) == 0) {
cb(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)
{
frame_callback = cb;
return true;
}