Ardupilot UAVCAN 学习《2》

目录

文章目录

  • 目录
  • 摘要
  • 1.CAN电调基础准备
    • 1.常用帧
    • 2.协议格式
    • 3.工作机制
    • 4 .UAVCAN ID
    • 5 .UAVCAN 波特率设置
    • 6 .UAVCAN 在ardupi++lot中打开的参数
  • 2.UAVCAN系统运维包括哪些内容电调初始java环境变量配置
  • 3.UAVi++++<4CAlinuxN电调周期执行函数
    • 1. 第一个函数canio_.receive(frame, deadline, flags)
    • 2.第二个函数handleLoopbackFrame(frame);
    • 3.调用第三个函数

摘要


本节继续梳理ardupilot的 UAVCAN代码。欢迎批系统/运维评指正!


之前写的ard初始化upilot uavcan 基础知识可以看:UAVCAN基础知识


1.CAN电调基础准备


1.常用帧

2.协议格java培训


                                            Ardupilot UAVCAN 学习《2》

3.工作机制


                                            Ardupilot UAVCAN 学习《2》

4 .UAVCAN ID


                                            Ardupilot UAVCAN 学习《2》

5 .UAVCAN系统运维工作内容 波特率设置


                                            Ardupilot UAVCAN 学习《2》
注意CAN通信的波特率是初始化失败是什么意思多少这个很重要。

6 .UAVCAN 在ardupilot中打开的参数


                                            Ardupilot UAVCAN 学习《2》


2.UAVCAN电调初始化


static THD_FUNCTION(main_loop,arg)
{
daemon_task = chThdGetSelfX();
#ifdef HAL_I2C_CLEAR_BUS
// Clear all I2C Buses. This can be needed on some boards which
// can get a stuck I2C peripheral on boot
//清除所有I2C总线。这可能需要在一些板上
//启动时会出现I2C外设卡滞
ChibiOS::I2CBus::clear_all();
#endif
//DMA配置
ChibiOS::Shared_DMA::init();
//power使能
peripheral_power_enable();
//使能USB
hal.uartA->begin(115200);
#ifdef HAL_SPI_CHECK_CLOCK_FREQ
// optional test of SPI clock frei++<4quencies
//SPI时钟频率的可选测试
ChibiOS::SPIDevice::test_clock_freq();
#endif 
hal.uartB->begin(115200); //GPS1波特率设置
hal.uartC->begin(115200); //修改波特率57600为115200
hal.uartG->begin(115200); //修改波特率57600为115200
hal.uartH->begin(57600);  //修改波特率57600为115200
hal.analogin->init();     //模拟端口初始化
hal.scheduler->init();    //任务调度初始化
/*****************************************************************
* 以低优先级运行setup()以确保CLI不会挂起系统,并允许初始传感器读取回路运行
*  run setup() at low priority to ensure CLI doesn't hang the
*  system, and to allow initial sensor read loops to run
******************************************************************/
hal_chibios_set_priority(APM_STARTUP_PRIORITY);
//初始化hal
schedulerInstance.hal_initialized();
//回调setup函数
g_callbacks->setup();
//系统初始化
hal.scheduler->system_initialized();
thread_running = true;
chRegSetThreadName(SKETCHNAME);
/***********************************************************
* 切换到主回路的高优先级
* switch to high priority for main loop
***********************************************************/
chThdSetPriority(APM_MAIN_PRIORITY);
while (true)
{
//调用loop()函数,主循环任务开始
g_callbacks->loop();
/*
give up 250 microseconds of time if the INS loop hasn't
called delay_microseconds_boost(), to ensure low priority
drivers get a ci++<4hance to run. Calling
delay_microseconds_boost() means we have already given up
time from the main loop, so we don't need to do it again
here
如果INS循环周期没有调用delay_microseconds_boost延迟函数,放弃250us时间,确保低优先级的驱动能
正常运转,号召delay_microseconds_boost意味着我们已经放弃主循环时间,因此我们不需要在做。
*/
if (!schedulerInstance.check_called_boost())
{
hal.scheduler->delay_microseconds(250);
}
}
thread_running = false;
}

通过 g_callbacks->loop();回掉初始化磁盘下面函数

void Copter::setup()
{
//加载var_info[]s中列出的变量的默认值---- Load the default values of variables listed in var_info[]s
AP_Param::setup_sketch_defaults();
//为直升机设置存储布局-------------------setup storage layout for copter
StorageManager::set_layout_copter();
//驱动设备初始化
init_ardupilot();
//初始化主循环计划程序------------------ initialise the main loop scheduler
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM);
}

调用 init_ardupilot();函数

void Copter::init_ardupilot()
{
......
//初始化CAN 总线
BoardConfig_CAN.init(); //apm板层can初始化函数
......
}

调用BoardConfig_CAN.init();

void AP_BoardConfig_CAN::init()
{
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) //最多支持两个can总线
{
_st_driver_number[i] = (int8_t) _var_info_can[i]._driver_number; //_driver_number驱动号
_st_can_debug[i] = (int8_t) _var_info_can[i]._can_debug; //_can_debug调试号
}
setup_canbus(); //初始化can总线
}

调用setup_canbus(linux创建文件); //初始化can总线

void AP_BoardConfig_CAN::setup_canbus(void)
{
//创建我们需要的所有驱动程序----- Create all drivers that we need
bool initret = true; //initret初始化
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++)
{
// Check the driver number assigned to this physical interface
//检查分配给此物理接口的驱动程序编号
uint8_t drv_num = _var_info_can[i]._driver_number;
if (drv_num != 0 && drv_num <= MAX_NUMBER_OF_CAN_DRIVERS)
{
if (hal.can_mgr[drv_num - 1] == nullptr)
{
// CAN Manager is the driver
// So if this driver was not created before for other physical interface - do it
//CAN管理器是驱动程序
//因此,如果这个驱动程序不是以前为其他物理接口创建的,请执行以下操作
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new PX4::PX4CANManager;
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new Linux::CANManager;
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
//创建对象
const_cast <AP_HAL::HAL&> (hal).can_mgr[drv_num - 1] = new ChibiOS::CANManager;
#endif
}
//对于这个现有的驱动程序(管理器),启动物理接口----- For this now existing driver (manager), start the physical interface
if (hal.can_mgr[drv_num - 1] != nullptr)
{
//设置传输波特率
initret &= hal.can_mgr[drv_num - 1]->begin(_var_info_can[i]._can_bitrate, i);
} else
{
printf("Failed to initialize can interface %d\n\r", i + 1);
}
}
}
bool any_uavcan_present = false;
//设置波特率完成初始化后,进入下面函数
if (initret)
{
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
{
if (hal.can_mgr[i] == nullptr)
{
continue;
}
hal.can_mgr[i]->initialized(true);
//初始化OK打印信息
printf("can_mgr %d initialized well\n\r", i + 1);
//检查协议是否初始化
if (_var_info_can_protocol[i]._protocol == UAVCAN_PROTOCOL_ENABLE)
{
//创建协议数组对象
_var_info_can_protocol[i]._uavcan = new AP_UAVCAN;
//创建对象不能为空
if (_var_info_can_protocol[i]._uavcan == nullptr)
{
AP_HAL::panic("Failed to allocate uavcan %d\n\r", i + 1);
continue;
}
//加载默认参数
AP_Param::load_object_from_eeprom(_var_info_can_protocol[i]._uavcan, AP_UAVCAN::var_info);
//设置协议
hal.can_mgr[i]->set_UAVCAN(_var_info_can_protocol[i]._uavcan);
_var_info_can_protocol[i]._uavcan->set_parent_can_mgr(hal.can_mgr[i]);
//尝试初始化
if (_var_info_can_protocol[i]._uavcan->try_init() == true)  //尝试初始化
{
any_uavcan_present = true;
} else
{
printf("Failed to initialize uavcan interface %d\n\r", i + 1);
}
}
}
//初始化设备完成
if (any_uavcan_present)
{
//开始UAVCAN工作线程 start UAVCAN working thread
hal.scheduler->create_uavcan_thread();
hal.console->printf("***CAN init Finish****\r\n");
// Delay for magnetometer and barometer discovery
//延迟地磁和气压计发现
hal.scheduler->delay(5000);
}
}
}

调用函数:_var_info_can_pi++和++iroi++<4tocol[i]._uavcan-&glinux系统安装t;try_initlinux必学的60个命令()

bool AP_UAVCAN::try_init(void)
{
//父类对象
if (_parent_can_mgr == nullptr)
{
return false;
}
//初始化OK吗
if (_initialized)
{
return true;
}
//对象初始化OK吗
if (!_parent_can_mgr->is_initialized())
{
return false;
}
_uavcan_i = UINT8_MAX;
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++)
{
if (_parent_can_mgr == hal.can_mgr[i])
{
_uavcan_i = i;
break;
}
}
if(_uavcan_i == UINT8_MAX)
{
return false;
}
//获取结点
auto *node = get_node();
if (node == nullptr)
{
return false;
}
//结点初始化OK
if (node->isStarted())
{
return false;
}
uavcan::NodeID self_node_id(_uavcan_node);
node->setNodeID(self_node_id);
char ndname[20];
snprintf(ndname, sizeof(ndname), "org.ardupilot:%u", _uavcan_i);
uavcan::NodeStatusProvider::NodeName name(ndname);
node->setName(name);
uavcan::protocol::SoftwareVersion sw_version; // Standard type uavcan.protocol.SoftwareVersion
sw_version.major = AP_UAVCAN_SW_VERS_MAJOR;
sw_version.minor = AP_UAVCAN_SW_VERS_MINOR;
node->setSoftwareVersion(sw_version);
uavcan::protocol::HardwareVersion hw_version; // Standard type uavcan.protocol.HardwareVersion
hw_version.major = AP_UAVCAN_HW_VERS_MAJOR;
hw_version.minor = AP_UAVCAN_HW_VERS_MINOR;
node->setHardwareVersion(hw_version);
const int node_start_res = node->start();
if (node_start_res < 0)
{
debug_uavcan(1, "UAVCAN: node start problem\n\r");
}
//    uavcan::Subscriber<uavcan::equipment::gnss::Fix> *gnss_fix;
//    gnss_fix = new uavcan::Subscriber<uavcan::equipment::gnss::Fix>(*node);
//
//    const int gnss_fix_start_res = gnss_fix->start(gnss_fix_cbi++<4_arr[_uavcan_i]);
//    if (gnss_fix_start_res < 0)
//    {
//        debug_uavcan(1, "UAVCAN GNSS subscriber start problem\n\r");
//        return false;
//    }
//    uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary> *gnss_aux;
//    gnss_aux = new uavcan::Subscriber<uavcan::equipment::gnss::Auxiliary>(*node);
//    const int gnss_aux_start_res = gnss_aux->start(gnss_aux_cb_arr[_uavcan_i]);
//    if (gnss_aux_start_res < 0)
//    {
//        debug_uavcan(1, "UAVCAN GNSS Aux subscriber start problem\n\r");
//        return false;
//    }
//    uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength> *magnetic;
//    magnetic = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength>(*node);
//    const int magnetic_start_res = magnetic->start(magnetic_cb_arr[_uavcan_i]);
//    if (magnetic_start_res < 0) {
//        debug_uavcan(1, "UAVCAN Compass subscriber start problem\n\r");
//        return false;
//    }
//
//    uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2> *magnetic2;
//    magnetic2 = new uavcan::Subscriber<uavcan::equipment::ahrs::MagneticFieldStrength2>(*node);
//    const int magnetic_start_res_2 = magnetic2->start(magnetic_cb_2_arr[_uavcan_i]);
//    if (magnetic_start_res_2 < 0) {
//        debug_uavcan(1, "UAVCAN Compass for multiple mags subscriber start problem\n\r");
//        return false;
//    }
//    uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure> *air_data_sp;
//    air_data_sp = new uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure>(*node);
//    const int air_data_sp_start_res = air_data_sp->start(air_data_sp_cb_arr[_uavcan_i]);
//    if (air_data_sp_start_res < 0) {
//        debug_uavcan(1, "UAVCAN Baro subscriber start problem\n\r");
//        return false;
//    }
//    uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature> *air_data_st;
//    air_data_st = new uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature>(*node);
//    const int air_data_st_start_res = air_data_st->start(air_data_st_cb_arr[_uavcan_i]);
//    if (air_data_st_start_res < 0) {
//        debug_uavcan(1, "UAVCAN Temperature subscriber start problem\n\r");
//        return false;
//    }
//    uavcan::Subscriber<uavcan::equipment::power::BatteryInfo> *battery_info_st;
//    battery_info_st = new uavcan::Subscriber<uavcan::equipment::power::BatteryInfo>(*node);
//    hal.console->printf("battery_info_st=%d\r\n",battery_info_st);
//    hal.console->printf("_uavcan_i=%d\r\n",_uavcan_i);
//    battery_info_st_cb_ari++<4r[_uavcan_i];
//    const int battery_info_start_res = battery_info_st->start(battery_info_st_cb_arr[_uavcan_i]); //获取数据battery_info_st_cb
//    if (battery_info_start_res < 0)
//    {
//        debug_uavcan(1, "UAVCAN BatteryInfo subscriber start problem\n\r");
//        return false;
//    }
uavcan::Subscriber<uavcan::equipment::esc::RPMCommand> *motor_esc_info_st;
motor_esc_info_st = new uavcan::Subscriber<uavcan::equipment::esc::RPMCommand>(*node);
const int motor_esc_info_start_res = motor_esc_info_st->start(motor_esc_info_st_cb_arr[_uavcan_i]); //获取数据battery_info_st_cb
if (motor_esc_info_start_res < 0)
{
hal.console->printf("UAVCAN Motor esc Info subscriber start problem\r\n");
debug_uavcan(1, "UAVCAN Motor esc Info subscriber start problem\n\r");
return false;
}
//这里需要注意,创建自己的uavcan设备类型
uavcan::Subscriber<uavcan::equipment::esc::INFCommand> *motor_esc_info_st1;
motor_esc_info_st1 = new uavcan::Subscriber<uavcan::equipment::esc::INFCommand>(*node);
const int motor_esc_info1_start_res = motor_esc_info_st1->start(motor_esc_info1_st_cb_arr[_uavcan_i]); //获取数据battery_info_st_cb
if (motor_esc_info1_start_res < 0)
{
hal.console->printf("UAVCAN Motor esc Info1 subscriber start problem\r\n");
debug_uavcan(1, "UAVCAN Motor esc Info subscriber start problem\n\r");
return false;
}
//    act_out_array[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::actuator::ArrayCommand>(*node);
//    act_out_array[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
//    act_out_array[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//
//    esc_raw[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::esc::RawCommand>(*node);
//    esc_raw[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
//    esc_raw[_uavcan_i]->setPriority(uavcan::TransferPriority::OneLowerThanHighest);
//    rgb_led[_uavcan_i] = new uavcan::Publisher<uavcan::equipment::indication::LightsCommand>(*node);
//    rgb_led[_uavcan_i]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(20));
//    rgb_ledi++<4[_uavcan_i]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
_led_conf.devices_count = 0;
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
node->setModeOperational();
_initialized = true;
debug_uavcan(1, "UAVCAN: init done\n\r");
return true;
}


                                            Ardupilot UAVCAN 学习《2》
注意:

static void (*motor_esc_info_st_cb_arr[2])(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand>& msg)
= { motor_esc_info_st_cb0, motor_esc_info_st_cb1 };

注意这两个回掉函数

static void motor_esc_info_st_cb0(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand>& msg)
{
motor_esc_info_st_cb(msg, 0);
}
static void motor_esc_info_st_cb1(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand>& msg)
{
motor_esc_info_st_cb(msg, 1);
}
static void motor_esc_info_st_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand>& msg, uint8_t mgr)
{
dzuav_motor_esc_rpm=msg.motor_esc_rpm;
dzuav_motor_esc_pwm=msg.motor_esc_pwm;
dzuav_motor_esc_status=msg.motor_esc_status;
}

找到
mot初始化电脑的后果or_esc_ilinux操作系统基础知识nfo_st->start()函数的linux删除文件命令执行函i++和++i的区别举例数start()函数linux,这个函数属于subscrilinux重启命令ber.hp初始化磁盘p文件中

    /**
* Begin receiving messages.
* Each message will be passed to the application via the callback.
* Returns negative error code.
*/
//*开始接收消息。每个消息都将通过回调传递给应用程序,返回负错误代码。
int start(const Callback& callback)
{
printf("######\r\n");
stop();
if (!coerceOrFallback<bool>(callback, true))
{
UAVCAN_TRACE("Subscriber", "Invalid callback");
return -ErrInvalidParam;
}
//具体如何实现回掉,可以仔细分析下这个函数
callback_ = callback;
printf("###123###\r\n");
return BaseType::startAsMessageListener();
}

3.UAVC数据漫游是什么意思AN电调周期执行函数


hal.scheduler->init();    //任务调度初始化
void Scheduler::init()
{
// setup the uavcan thread - this will call tasks at 1kHz
//设置uavcan线程-这将调用1kHz的任务
#if HAL_WITH_UAVCAN
_uavcan_thread_ctx = chThdCreateStatic(_uavcan_thread_wa,
sizeof(_uavcan_thread_wa),
APM_UAVCAN_PRIORITY,        /* 初始化优先级*/
_uavcan_thread,            /* 线程函数 */
this);                    /* 线程参数*/
#endif
}

调用线程函数_uavcan_thread

#if HAL_WITH_UAVCAN
void Scheduler::_uavcan_thread(void *arg)
{
Scheduler *sched = (Scheduler *)arg;
chRegSetThreadName("apm_uavcan");
while (!sched->_hal_initialized)
{
sched->delay_microseconds(20000);
}
while (true)
{
sched->delay_microseconds(300);
for (int i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++)
{
if (AP_UAVCAN::get_uavcan(i) != nullptr)
{
//调用定时器函数
CANManager::from(hal.can_mgr[i])->_timer_tick();
}
}
}
}
#endif

调用_tim初始化电脑的后果er_tick 1HZ的定时器函数

void CANManager::_timer_tick()
{
if (!initialized_) return;
if (p_uavcan != nullptr)
{
p_uavcan->do_cyclic(); //循环处理
}
}

调用 p_uavcan->do_cyclic(); //循环处理函数

void AP_UAVCAN::do_cyclic(void)
{
if (!_initialized)
{
hal.scheduler->delay_microseconds(1000);
return;
}
//获取结点
auto *node = get_node();
const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));  //获取解析数据
if (error < 0)
{
hal.scheduler->delay_microseconds(100);
return;
}
//判断是否解锁
if (_SRV_armed)
{
bool sent_servos = false;
if (_servo_bm > 0)
{
// if we have any Servos in bitmask
uint32_t now = AP_HAL::micros();
const uint32_t servo_period_us = 1000000UL / unsigned(_servo_rate_hz.get());
if (now - _SRV_last_send_us >= servo_period_us)
{
_SRV_last_send_us = now;
SRV_send_servos();
sent_servos = true;
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++)
{
_SRV_conf[i].servo_pending = false;
}
}
}
//如果我们有别的ESC电调标记位---- if we have any ESC's in bitmask
if (_esc_bm > 0 && !sent_servos)
{
//发送电调数据
SRV_send_esc();
}
for (uint8_t i = 0; i < UAVCAN_SRV_NUMBER; i++)
{
_SRV_conf[i].esc_i++<4pending = false;
}
}
//led的数据处理
if (led_out_sem_take())
{
led_out_send();
led_out_sem_give();
}
}

调用

const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1));  //获取解析数据
    //调用函数
int spin(MonotonicDuration duration)
{
if (started_)
{
return INode::spin(duration); //执行
}
return -ErrNotInited;
}
int Scheduler::spin(MonotonicTime deadline)
{
if (inside_spin_)  // Preventing recursive calls
{
UAVCAN_ASSERT(0);
return -ErrRecursiveCall;
}
InsideSpinSetter iss(*this);
UAVCAN_ASSERT(inside_spin_);
int retval = 0;
while (true)
{
const MonotonicTime dl = computeDispatcherSpinDeadline(deadline);
retval = dispatcher_.spin(dl); //执行
if (retval < 0)
{
break;
}
const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
pollCleanup(ts, unsigned(retval));
if (ts >= deadline)
{
break;
}
}
return retval;
}
int Dispatcher::spin(MonotonicTime deadline)
{
int num_frames_processed = 0;
do
{
CanIOFlags flags = 0;
CanRxFrame frame;
const int res = canio_.receive(frame, deadline, flags);
if (res < 0)
{
return res;
}
if (res > 0)
{
if (flags & CanIOFlagLoopback)
{
handleLoopbackFrame(frame);
}
else
{
num_frames_processed++;
// printf("num_frames_processed=%d\r\n",num_frames_processed);//res=1
handleFrame(frame);
}
notifyRxFrameListener(frame, flags);
}
}
while (sysclock_.getMonotonic() < deadline);
return num_frames_processed;
}

这里需要注意的函数

const int res = canio_.receive(frame, deadline, flags);
handleLoopbackFrame(frame);
handleFrame(frame);

其中:CanIOManager canio_;系统运维面试题及答案


1. 第一个函数canio_.recelinux常用命令ive(frame, deadline, flags)


int CanIOManager::receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags)
{
const uint8_t num_ifaces = getNumIfaces();
// printf("num_ifaces=%d\r\n",num_ifaces);
while (true)
{
CanSelectMasks masks;
masks.write = makePendingTxMask();
masks.read = uint8_t((1 << num_ifaces) - 1);
{
const CanFrame* pending_tx[MaxCanIfaces] = {};
for (int i = 0; i < num_ifaces; i++)      // Dear compiler, kindly unroll this. Thanks.
{
pending_tx[i] = tx_queues_[i]->getTopPriorityPendingFrame();
}
// printf("blocking_deadline=%d\r\n",blocking_deadline);
const int select_res = callSelect(masks, pending_tx, blocking_deadline);
//            printf("select_res=%d\r\n",select_res);
if (select_res < 0)
{
return -ErrDriver;
}
}
// Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (masks.write & (1 << i))
{
(void)sendFromTxQueue(i);  // It may fail, we don't care. Requested operation was receive, not send.
}
}
// Read
for (uint8_t i = 0; i < num_ifaces; i++)
{
if (masks.read & (1 << i))
{
ICanIface* const iface = driver_.getIface(i);
if (iface == UAVCAN_NULLPTR)
{
UAVCAN_ASSERT(0);   // Nonexistent interface
continuei++<4;
}
//                printf("out_frame=%d\r\n",out_frame);
//                printf("out_frame.ts_mono=%d\r\n",out_frame.ts_mono);
//                printf("out_frame.ts_utc=%d\r\n",out_frame.ts_utc);
//                printf("out_flags=%d\r\n",out_flags);
const int res = iface->receive(out_frame, out_frame.ts_mono, out_frame.ts_utc, out_flags);
//  printf("res=%d\r\n",res);
if (res == 0)
{
UAVCAN_ASSERT(0);   // select() reported that iface has pending RX frames, but receive() returned none
continue;
}
out_frame.iface_index = i;
if ((res > 0) && !(out_flags & CanIOFlagLoopback))
{
counters_[i].frames_rx += 1;
}
return (res < 0) ? -ErrDriver : res;
}
}
// Timeout checked in the last order - this way we can operate with expired deadline:
if (sysclock_.getMonotonic() >= blocking_deadline)
{
break;
}
}
return 0;
}

接收函数在modules/uavcan/libuavcan_drivers/stm32/driv数据er/src/uc_stm32_can.cpp


uavcan::int16_t CanIface::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
{
out_ts_monotonic = clock::getMonotonic();  // High precision is not required for monotonic timestamps
uavcan::uint64_t utc_usec = 0;
{
CriticalSectionLocker lock;
if (rx_queue_.getLength() == 0)
{
return 0;
}
rx_queue_.pop(out_frame, utc_usec, out_flags);
}
out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
return 1;
}

注意这个函数
const int select_res = callSelect(masks, pending_tx, blocking_deadline);

int CanIOManager::callSelect(CanSelectMasks& inout_masks, const CanFrame* (& pending_tx)[MaxCanIfaces],
MonotonicTime blocking_deadline)
{
const CanSelectMasks in_masks = inout_masks;
const int res = driver_.select(inout_masks, pending_tx, blocking_deadline);
if (res < 0)
{
return -ErrDriver;
}
inout_masks.read  &= in_masks.read;  // Driver is not required to clean the masks
inout_masks.write &= in_masks.write;
return res;
}

注意函数
//选择读写
const int res = driver_.select(inout_masks, pending_tx, blocking_deadline);

    virtual int16_t select(CanSelectMasks& inout_masks,
const CanFrame* (& pending_tx)[MaxCanIfaces],
MonotonicTime blocking_deadline) = 0;
uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
const uavcan::MonotonicTime blocking_deadline)
{
const uavcan::CanSelectMasks in_masks = inout_masks;
const uavcan::MonotonicTime time = clock::getMonotonic();
if0_.discardTimedOutTxMailboxes(time);              // Check TX timeouts - this may release some TX slots
{
CriticalSectionLocker cs_locker;
if0_.pollErrorFlagsFromISR();
}
#if UAVCAN_STM32_NUM_IFACES > 1
if1_.discardTimedOutTxMailboxes(time);
{
CriticalSectionLocker cs_locker;
if1_.pollErrorFlagsFromISR();
}
#endif
inout_masks = makeSelectMasks(pending_tx);          // Check if we already have some of the requested events
if ((inout_masks.read  & in_masks.read)  != 0 ||
(inout_masks.write & in_masks.write) != 0)
{
return 1;
}
(void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates
inout_masks = makeSelectMasks(pending_tx);  // Return what we got even if none of the requested events are set
return 1;                                   // Return value doesn't matter as long as it is non-negative
}

2.第二个函数handleLoopbackFrame(f数据ra初始化失败是怎么解决me);

void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame)
{
RxFrame frame;
//解析接收到的数据
if (!frame.parse(can_frame))
{
UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str());
UAVCAN_ASSERT(0);  // No way!
return;
}
UAVCAN_ASSERT(frame.getSrcNodeID() == getNodeID());
loopback_listeners_.invokeListeners(frame);
}

首先调用RxFrame的解析函数

bool RxFrame::parse(const CanRxFrame& can_frame)
{
//这里在调用解析函数
if (!Frame::parse(can_frame))
{
return false;
}
if (can_frame.ts_mono.isZero())  // Monotonic timestamps are mandatory.
{
UAVCAN_ASSERT(0);                   // If it is not set, it's a driver failure.
return false;
}
ts_mono_ = can_frame.ts_mono;
ts_utc_ = can_frame.ts_utc;
iface_index_ = can_frame.iface_index;
return true;
}
bool Frame::parse(const CanFrame& can_frame)
{
static uint8_t num=0;
if (can_frame.isErrorFrame() || can_frame.isRemoteTransmissionRequest() || !can_frame.isExtended())
{
UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
return false;
}
num++;
switch (num)
{
case 1:
//    	printf("num=%d\r\n",num);
battery_volt[0]=can_frame.data[6]; //获取电池电压的低位数据
break;
case 2:
//    	printf("num=%d\r\n",num);
mydata[0]=1;
battery_volt[1]=can_frame.data[0]; //获取电池电压的高位数据
battery_current[0]=can_frame.data[1]; //获取电池电流的低位数据
battery_current[1]=can_frame.data[2]; //获取电池电流的高位数据
battery_temperature[0]=can_frame.data[3]; //获取电池温度的低位数据
battery_temperature[1]=can_frame.data[4]; //获取电池温度的高位数据
battery_capability_percent[0]=can_frame.data[5]; //获取剩余电量百分比
battery_capability_percent[1]=can_frame.data[6]; //获取剩余电量百分比
//最终的电池电压
battery_volt_inf=battery_volt[1]*256+battery_volt[0];
//    	mycan_frame.batteri++<4y_volt_inf=battery_volt_inf;
mydata[0]=battery_volt_inf;
//最终电流
battery_current_inf=battery_current[1]*256+battery_current[0];
//    	mycan_frame.battery_current_inf=battery_current_inf;
//最终温度
battery_temp_inf=battery_temperature[1]*256+battery_temperature[0];
//电池电量百分比
battery_capabi_percent=battery_capability_percent[1]*256+battery_capability_percent[0];
break;
case 7:
battery_residual_capability[0]=can_frame.data[2]; //获取剩余容量
battery_residual_capability[1]=can_frame.data[3]; //获取剩余容量
battery_res_capability=battery_residual_capability[1]*256+battery_residual_capability[0];
break;
default:
break;
}
if(num)
{
if(num==8)
{
num=0;
}
//    	printf("battery_volt_inf =%lu\r\n",battery_volt_inf );
//    	printf("battery_current_inf =%lu\r\n",battery_current_inf );
//    	printf("battery_temp_inf =%lu\r\n",battery_temp_inf );
//    	printf("battery_volt_inf =%lu\r\n",battery_volt_inf );
//    	printf("battery_capabi_percent =%lu\r\n",battery_capabi_percent );
//    	printf("battery_res_capability =%lu\r\n",battery_res_capability );
//        printf("can_frame.dlc =%d\r\n",can_frame.dlc );
//        printf("can_frame.id =%lu\r\n",can_frame.id );
//        printf("can_frame.data[0] =%d\r\n",can_frame.data[0]);
//        printf("can_frame.data[1] =%d\r\n",can_frame.data[1]);
//        printf("can_frame.data[2] =%d\r\n",can_frame.data[2]);
//        printf("can_framei++<4.data[3] =%d\r\n",can_frame.data[3]);
//        printf("can_frame.data[4] =%d\r\n",can_frame.data[4]);
//        printf("can_frame.data[5] =%d\r\n",can_frame.data[5]);
//        printf("can_frame.data[6] =%d\r\n",can_frame.data[6]);
//        printf("can_frame.data[7] =%d\r\n",can_frame.data[7]);
}
//    printf("sizeof(can_frame.data) =%d\r\n",sizeof(can_frame.data) );
if (can_frame.dlc > sizeof(can_frame.data))
{
UAVCAN_ASSERT(0);  // This is not a protocol error, so UAVCAN_ASSERT() is ok
return false;
}
if (can_frame.dlc < 1)
{
UAVCAN_TRACE("Frame", "Parsing failed at line %d", __LINE__);
return false;
}
/*
* CAN ID parsing----解析can数据
*/
const uint32_t id = can_frame.id & CanFrame::MaskExtID;
//    printf("id =%lu\r\n",id);
transfer_priority_ = static_cast<uint8_t>(bitunpack<24, 5>(id));
src_node_id_ = static_cast<uint8_t>(bitunpack<0, 7>(id));
//    printf("transfer_priority_ =%lu\r\n",transfer_priority_); //transfer_priority_=1
//    printf("src_node_id_ =%lu\r\n",src_node_id_); //src_node_id_=22
const bool service_not_message = bitunpack<7, 1>(id) != 0U;
//    printf("service_not_message =%d\r\n",service_not_message); //service_not_message=0
if (service_not_message)
{
const bool request_not_response = bitunpack<15, 1>(id) != 0U;
transfer_type_ = request_not_response ? TransferTypeServiceRequest : TransferTypeServiceResponse;
dst_node_id_ = static_cast<uint8_t>(bitunpack<8, 7>(id));
data_type_id_ = static_cast<uint16_t>(bitunpack<16, 8>(id));
}
else
{
transfer_type_ = TransferTypeMessageBroadcast;
dst_node_id_ = NodeID::Broadcast;
data_type_id_ = static_cast<uint16_t>(bitunpack<8, 16>(id));
//        printf("transfer_type_ =%lu\r\n",transfer_type_); //transfer_type_=2
//        printf("dst_node_id_ =%lu\r\n",dst_node_id_);    //dst_node_id_=0
//        printf("data_type_id_ =%lu\r\n",data_type_id_);   //data_type_id_=4242就是0x1092
//        printf("src_node_id_.isBroadcast() =%d\r\n",src_nodi++<4e_id_.isBroadcast());   //
if (src_node_id_.isBroadcast())//src_node_id_.isBroadcast()=0
{
// Removing the discriminator
data_type_id_ = static_cast<uint16_t>(data_type_id_.get() & 3U);
//            printf("data_type_id_ =%lu\r\n",data_type_id_);   //
}
}
/*
* CAN payload parsing
*/
payload_len_ = static_cast<uint8_t>(can_frame.dlc - 1U);
//    printf("payload_len_ =%d\r\n",payload_len_);   //
(void)copy(can_frame.data, can_frame.data + payload_len_, payload_); //把数据已经拷贝到payload_这里面
//    printf("payload_[0]=%d\r\n",payload_[0]);
//    printf("payload_[1]=%d\r\n",payload_[1]);
//    printf("payload_[2]=%d\r\n",payload_[2]);
//    printf("payload_[3]=%d\r\n",payload_[3]);
//    printf("payload_[4]=%d\r\n",payload_[4]);
//    printf("payload_[5]=%d\r\n",payload_[5]);
//    printf("payload_[6]=%d\r\n",payload_[6]);
const uint8_t tail = can_frame.data[can_frame.dlc - 1U];
//    printf("can_frame.dlc =%d\r\n",can_frame.dlc);   //
//    printf("tail =%d\r\n",tail);
//    printf("can_frame.data[can_frame.dlc - 1U] =%d\r\n",can_frame.data[can_frame.dlc - 1U]);//start_of_transfer_=0
start_of_transfer_ = (tail & (1U << 7)) != 0;
end_of_transfer_   = (tail & (1U << 6)) != 0;
toggle_            = (tail & (1U << 5)) != 0;
transfer_id_ = tail & TransferID::Max;
//   printf("start_of_transfer_ =%d\r\n",start_of_transfer_);//start_of_transfer_=0
//    printf("end_of_transfer_ =%d\r\n",end_of_transfer_);//end_of_transfer_=1
//    printf("toggle_ =%d\r\n",toggle_);//toggle_=1
//    printf("transfer_id_ =%d\r\n",transfer_id_); //transfer_id_=7
return isValid();
}

3.调用第三个函系统运维工作内容


void Dispatcher::handleFrame(const CanRxFrame& can_frame)

void Dispatcher::handleFrame(const CanRxFrame& can_frame)
{
RxFrame frame;
if (!frame.parse(can_frame)) //解析数据,这里已经获取到数据can_frame.data[0]~can_frame.data[7]
{
printf("HJJ\r\n" );
// This is not counted as a transport error
UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
return;
}
printf("frame.getDstNodeID() =%d\r\n",frame.getDstNodeID() ); //frame.getDstNodeID()=0
printf("NodeID::Broadcast =%d\r\n",NodeID::Broadcast);//NodeID::Broadcast =0
printf("getNodeID() =%d\r\n",getNodeID() );//getNodeID()=10
if ((frame.getDstNodeID() != NodeID::Broadcast) &&(frame.getDstNodeID() != getNodeID()))
{
printf("HJJ1\r\n" );
return;
}
printfi++<4("frame.getTransferType() =%d\r\n",frame.getTransferType() );//frame.getTransferType() =2
switch (frame.getTransferType())
{
case TransferTypeMessageBroadcast:
{
lmsg_.handleFrame(frame); //进行数据处理
break;
}
case TransferTypeServiceRequest:
{
lsrv_req_.handleFrame(frame);
break;
}
case TransferTypeServiceResponse:
{
lsrv_resp_.handleFrame(frame);
break;
}
default:
{
UAVCAN_ASSERT(0);
break;
}
}
}

函数:lmsg_.handleFrame初始化磁盘(frame); //进行数据处理

void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame)
{
TransferListener* p = list_.get();
printf("p  =%d\r\n",p);
printf("frame.getDataTypeID()  =%lu\r\n",frame.getDataTypeID());
while (p)
{
TransferListener* const next = p->getNextListNode();
//frame.getTransferType() =20050
printf("frame.getDataTypeID()  =%lu\r\n",frame.getDataTypeID());
printf("p->getDataTypeDescriptor().getID()  =%lu\r\n",p->getDataTypeDescriptor().getID());//frame.getTransferType() =2
if (p->getDataTypeDescriptor().getID() == frame.getDataTypeID())
{
printf("FFF\r\n");//frame.getTransferType() =2
p->handleFrame(frame); // p may be modified
}
else if (p->getDataTypeDescriptor().getID() < frame.getDataTypeID())  // Listeners are ordered by data type id!
{
printf("DDD000\r\n");//frame.getTransferType() =2
break;
}
else
{
printf("SSS\r\n");//frame.getTransferType() =2
//            ;  // Nothing to do with this one
}
p = next;
}
}