aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL: add CANIface HAL, replace uavcan dependent one

Browse Source
This commit is contained in:
Siddharth Purohit 2020-05-31 17:42:02 +05:30 committed by Andrew Tridgell
parent 697e4141cb
commit fa0f1e4c71
11 changed files with 300 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_HAL/AP_HAL.h
View File

@ -22,9 +22,7 @@
#include "Flash.h"
#include "DSP.h"
#if HAL_WITH_UAVCAN
#include "CAN.h"
#endif
#include "CANIface.h"
#include "utility/BetterStream.h"

14
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -156,8 +156,8 @@
#define HAL_COMPASS_HMC5843_I2C_ADDR 0x1E
#endif
#ifndef HAL_WITH_UAVCAN
#define HAL_WITH_UAVCAN 0
#ifndef HAL_NUM_CAN_IFACES
#define HAL_NUM_CAN_IFACES 0
#endif
#ifndef HAL_RCINPUT_WITH_AP_RADIO
@ -215,6 +215,14 @@
#define HAL_CAN_DRIVER_DEFAULT 0
#endif
#ifndef HAL_MAX_CAN_PROTOCOL_DRIVERS
#define HAL_MAX_CAN_PROTOCOL_DRIVERS HAL_NUM_CAN_IFACES
#endif
#ifndef HAL_ENABLE_LIBUAVCAN_DRIVERS
#define HAL_ENABLE_LIBUAVCAN_DRIVERS (HAL_MAX_CAN_PROTOCOL_DRIVERS > 0)
#endif
#ifdef HAVE_LIBDL
#define AP_MODULE_SUPPORTED 1
#else
@ -230,7 +238,7 @@
#define HAL_HAVE_DUAL_USB_CDC 0
#endif
#if HAL_WITH_UAVCAN && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#if HAL_NUM_CAN_IFACES && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
#define AP_UAVCAN_SLCAN_ENABLED 1
#else
#define AP_UAVCAN_SLCAN_ENABLED 0

2
libraries/AP_HAL/AP_HAL_Macros.h
View File

@ -40,7 +40,7 @@
#define floor(x) DO_NOT_USE_DOUBLE_MATHS()
#define round(x) DO_NOT_USE_DOUBLE_MATHS()
#define fmax(x,y) DO_NOT_USE_DOUBLE_MATHS()
#if !HAL_WITH_UAVCAN
#if !HAL_NUM_CAN_IFACES
// we should do log() and fabs() as well, but can't because of a conflict in uavcan
#define log(x) DO_NOT_USE_DOUBLE_MATHS()
#define fabs(x) DO_NOT_USE_DOUBLE_MATHS()

5
libraries/AP_HAL/AP_HAL_Namespace.h
View File

@ -32,9 +32,8 @@ namespace AP_HAL {
class OpticalFlow;
class DSP;
class CANProtocol;
class CANManager;
class CANHal;
class CANIface;
class CANFrame;
class Util;
class Flash;

134
libraries/AP_HAL/CAN.h
View File

@ -1,134 +0,0 @@
/*
* Copyright (C) 2017 Eugene Shamaev
*
* 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/>.
*/
#pragma once
#include <inttypes.h>
#include <string.h>
#include <assert.h>
#include <cmath>
#if HAL_WITH_UAVCAN
#include "AP_HAL_Namespace.h"
#include "utility/functor.h"
#include <uavcan/driver/can.hpp>
#include <uavcan/time.hpp>
#define MAX_NUMBER_OF_CAN_INTERFACES 2
#define MAX_NUMBER_OF_CAN_DRIVERS 2
/**
* Interface that CAN protocols need to implement
*/
class AP_HAL::CANProtocol {
public:
/* method called when initializing the CAN interfaces
*
* if initialization doesn't have errors, protocol class
* should create a thread to do send and receive operations
*/
virtual void init(uint8_t driver_index, bool enable_filters) = 0;
};
/**
* Single non-blocking CAN interface.
*/
class AP_HAL::CANHal: public uavcan::ICanIface {
public:
/* CAN port open method
bitrate Selects the speed that the port will be configured to. If zero, the port speed is left unchanged.
return false - CAN port open failed
true - CAN port open succeeded
*/
virtual bool begin(uint32_t bitrate) = 0;
/*
CAN port close
*/
virtual void end() = 0;
/*
Reset opened CAN port
Pending messages to be transmitted are deleted and receive state and FIFO also reset.
All pending errors are cleared.
*/
virtual void reset() = 0;
/*
Test if CAN port is opened and initialized
return false - CAN port not initialized
true - CAN port is initialized
*/
virtual bool is_initialized() = 0;
/*
Return if CAN port has some untransmitted pending messages
return -1 - CAN port is not opened or initialized
0 - no messages are pending
positive - number of pending messages
*/
virtual int32_t tx_pending() = 0;
/*
Return if CAN port has received but not yet read messages
return -1 - CAN port is not opened or initialized
0 - no messages are pending for read
positive - number of pending messages for read
*/
virtual int32_t available() = 0;
};
/**
* Generic CAN driver.
*/
class AP_HAL::CANManager {
public:
CANManager(uavcan::ICanDriver* driver) : _driver(driver) {}
/* CAN port open method
Opens port with specified bit rate
bitrate - selects the speed that the port will be configured to. If zero, the port speed is left
unchanged.
can_number - the index of can interface to be opened
return false - CAN port open failed
true - CAN port open succeeded
*/
virtual bool begin(uint32_t bitrate, uint8_t can_number) = 0;
/*
Test if CAN manager is ready and initialized
return false - CAN manager not initialized
true - CAN manager is initialized
*/
virtual bool is_initialized() = 0;
virtual void initialized(bool val) = 0;
uavcan::ICanDriver* get_driver() { return _driver; }
private:
uavcan::ICanDriver* _driver;
};
#endif

53
libraries/AP_HAL/CANIface.cpp Normal file
View File

@ -0,0 +1,53 @@
/*
* 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"
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;
}

209
libraries/AP_HAL/CANIface.h Normal file
View File

@ -0,0 +1,209 @@
/*
* 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/>.
*/
#pragma once
#include <stdint.h>
#include "AP_HAL_Namespace.h"
/**
* Raw CAN frame, as passed to/from the CAN driver.
*/
struct AP_HAL::CANFrame {
static const uint32_t MaskStdID = 0x000007FFU;
static const uint32_t MaskExtID = 0x1FFFFFFFU;
static const uint32_t FlagEFF = 1U << 31; ///< Extended frame format
static const uint32_t FlagRTR = 1U << 30; ///< Remote transmission request
static const uint32_t FlagERR = 1U << 29; ///< Error frame
static const uint8_t MaxDataLen = 8;
uint32_t id; ///< CAN ID with flags (above)
uint8_t data[MaxDataLen];
uint8_t dlc; ///< Data Length Code
CANFrame() :
id(0),
dlc(0)
{
memset(data,0, MaxDataLen);
}
CANFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) :
id(can_id),
dlc((data_len > MaxDataLen) ? MaxDataLen : data_len)
{
if ((can_data == nullptr) || (data_len != dlc) || (dlc == 0)) {
return;
}
memcpy(this->data, can_data, dlc);
}
bool operator!=(const CANFrame& rhs) const
{
return !operator==(rhs);
}
bool operator==(const CANFrame& rhs) const
{
return (id == rhs.id) && (dlc == rhs.dlc) && (memcmp(data, rhs.data, dlc) == 0);
}
bool isExtended() const
{
return id & FlagEFF;
}
bool isRemoteTransmissionRequest() const
{
return id & FlagRTR;
}
bool isErrorFrame() const
{
return id & FlagERR;
}
/**
* CAN frame arbitration rules, particularly STD vs EXT:
* Marco Di Natale - "Understanding and using the Controller Area Network"
* http://www6.in.tum.de/pub/Main/TeachingWs2013MSE/CANbus.pdf
*/
bool priorityHigherThan(const CANFrame& rhs) const;
bool priorityLowerThan(const CANFrame& rhs) const
{
return rhs.priorityHigherThan(*this);
}
};
class AP_HAL::CANIface
{
public:
enum OperatingMode {
PassThroughMode,
NormalMode,
SilentMode
};
typedef uint16_t CanIOFlags;
static const CanIOFlags Loopback = 1;
static const CanIOFlags AbortOnError = 2;
// Single Rx Frame with related info
struct CanRxItem {
uint64_t timestamp_us = 0;
CanIOFlags flags = 0;
CANFrame frame;
};
// Single Tx Frame with related info
struct CanTxItem {
uint64_t deadline = 0;
CANFrame frame;
uint32_t index = 0;
bool loopback = false;
bool abort_on_error = false;
bool aborted = false;
bool pushed = false;
bool setup = false;
bool operator<(const CanTxItem& rhs) const
{
if (frame.priorityLowerThan(rhs.frame)) {
return true;
}
if (frame.priorityHigherThan(rhs.frame)) {
return false;
}
return index > rhs.index;
}
};
struct CanFilterConfig {
uint32_t id = 0;
uint32_t mask = 0;
bool operator==(const CanFilterConfig& rhs) const
{
return rhs.id == id && rhs.mask == mask;
}
};
// Initialise the interface with hardware configuration required to start comms.
virtual bool init(const uint32_t bitrate, const OperatingMode mode) = 0;
// Select method to notify user of Rx and Tx buffer state.
// fill read select with true if a frame is available in Rx buffer
// fill write select with true if space is available in Tx buffer
// Also waits for Rx or Tx event depending on read_select and write_select values
// passed to the method until timeout. Returns true if the Rx/Tx even occured
// while waiting, false if timedout
virtual bool select(bool &read_select, bool &write_select,
const CANFrame* const pending_tx, uint64_t timeout)
{
return false;
}
virtual bool set_event_handle(EventHandle* evt_handle)
{
return true;
}
// Put frame in queue to be sent, return negative if error occured, 0 if no space, and 1 if successful
virtual int16_t send(const CANFrame& frame, uint64_t tx_deadline, CanIOFlags flags) = 0;
// Non blocking receive frame that pops the frames received inside the buffer, return negative if error occured,
// 0 if no frame available, 1 if successful
virtual int16_t receive(CANFrame& out_frame, uint64_t& out_ts_monotonic, CanIOFlags& out_flags) = 0;
//Configure filters so as to reject frames that are not going to be handled by us
virtual bool configureFilters(const CanFilterConfig* filter_configs, uint16_t num_configs)
{
return 0;
}
//Number of available hardware filters.
virtual uint16_t getNumFilters() const
{
return 0;
}
//Return Total Error Count generated so far
virtual uint32_t getErrorCount() const
{
return 0;
}
//Get status info of the interface
virtual uint32_t get_stats(char* data, uint32_t max_size)
{
return 0;
}
// return true if busoff was detected and not cleared
virtual bool is_busoff() const
{
return false;
}
// Methods to be used only while testing CANBus
// Not for normal operation or use case
virtual void flush_tx() {}
virtual void clear_rx() {}
// return true if init was called and successful
virtual bool is_initialized() const = 0;
};

28
libraries/AP_HAL/HAL.h
View File

@ -14,9 +14,7 @@ class AP_Param;
#include "system.h"
#include "OpticalFlow.h"
#include "DSP.h"
#if HAL_WITH_UAVCAN
#include "CAN.h"
#endif
#include "CANIface.h"
class AP_HAL::HAL {
@ -42,10 +40,10 @@ public:
AP_HAL::OpticalFlow*_opticalflow,
AP_HAL::Flash* _flash,
AP_HAL::DSP* _dsp,
#if HAL_WITH_UAVCAN
AP_HAL::CANManager* _can_mgr[MAX_NUMBER_OF_CAN_DRIVERS])
#if HAL_NUM_CAN_IFACES > 0
AP_HAL::CANIface* _can_ifaces[HAL_NUM_CAN_IFACES])
#else
AP_HAL::CANManager** _can_mgr)
AP_HAL::CANIface** _can_ifaces)
#endif
:
uartA(_uartA),
@ -70,13 +68,13 @@ public:
flash(_flash),
dsp(_dsp)
{
#if HAL_WITH_UAVCAN
if (_can_mgr == nullptr) {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
can_mgr[i] = nullptr;
#if HAL_NUM_CAN_IFACES > 0
if (_can_ifaces == nullptr) {
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
can[i] = nullptr;
} else {
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
can_mgr[i] = _can_mgr[i];
for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++)
can[i] = _can_ifaces[i];
}
#endif
@ -122,9 +120,9 @@ public:
AP_HAL::OpticalFlow *opticalflow;
AP_HAL::Flash *flash;
AP_HAL::DSP *dsp;
#if HAL_WITH_UAVCAN
AP_HAL::CANManager* can_mgr[MAX_NUMBER_OF_CAN_DRIVERS];
#if HAL_NUM_CAN_IFACES > 0
AP_HAL::CANIface* can[HAL_NUM_CAN_IFACES];
#else
AP_HAL::CANManager** can_mgr;
AP_HAL::CANIface** can;
#endif
};

4
libraries/AP_HAL/board/chibios.h
View File

@ -26,8 +26,8 @@
#define HAL_GPIO_LED_OFF 1
#endif
#ifndef HAL_WITH_UAVCAN
#define HAL_WITH_UAVCAN 0
#ifndef HAL_NUM_CAN_IFACES
#define HAL_NUM_CAN_IFACES 0
#endif
#ifndef HAL_HAVE_BOARD_VOLTAGE

10
libraries/AP_HAL/board/linux.h
View File

@ -175,7 +175,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 2
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 2
#define HAL_WITH_UAVCAN 1
#define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(LSM9DS1, "lsm9ds1_ag", ROTATION_PITCH_180)
#define HAL_MAG_PROBE_LIST PROBE_MAG_SPI(LSM9DS1, "lsm9ds1_m", ROTATION_NONE)
@ -199,7 +199,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 1
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 1
#define HAL_WITH_UAVCAN 1
#define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET
#define HAL_GPIO_A_LED_PIN 59
#define HAL_GPIO_B_LED_PIN 58
@ -213,7 +213,7 @@
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_OPTFLOW_PX4FLOW_I2C_BUS 2
#define HAL_RANGEFINDER_LIGHTWARE_I2C_BUS 2
#define HAL_WITH_UAVCAN 1
#define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH
#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77)
#define HAL_INS_PROBE1 PROBE_IMU_I2C(Invensense, 1, 0x69, ROTATION_NONE)
@ -247,7 +247,7 @@
#define HAL_MAG_PROBE3 PROBE_MAG_I2C(IST8310, 4, 0x0e, true, ROTATION_PITCH_180_YAW_90)
#define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2; HAL_MAG_PROBE3
#define HAL_RCOUTPUT_TAP_DEVICE "/dev/ttyS1"
#define HAL_WITH_UAVCAN 1
#define HAL_NUM_CAN_IFACES 1
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK
#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE)
#define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE)
@ -268,7 +268,7 @@
// only external compasses
#define HAL_PROBE_EXTERNAL_I2C_COMPASSES
#define HAL_COMPASS_DEFAULT HAL_COMPASS_NONE
#define HAL_WITH_UAVCAN 1
#define HAL_NUM_CAN_IFACES 1
#define HAL_IMU_TEMP_DEFAULT 55
#define HAL_HAVE_IMU_HEATER 1
#define HAL_UTILS_HEAT HAL_LINUX_HEAT_PWM

4
libraries/AP_HAL/board/sitl.h
View File

@ -61,7 +61,9 @@
#include <AP_HAL/EventHandle.h>
#define HAL_EventHandle AP_HAL::EventHandle
#define HAL_NUM_CAN_IFACES 2
#ifndef HAL_NUM_CAN_IFACES
#define HAL_NUM_CAN_IFACES 0
#endif
#ifndef HAL_BOARD_STORAGE_DIRECTORY
#define HAL_BOARD_STORAGE_DIRECTORY "."