preempt 内核配置

已于 2022-10-12 18:25:44 修改
阅读量616 收藏
点赞数
文章标签： linux ubuntu 运维
于 2022-10-12 18:01:50 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zlf19910726/article/details/127287734
版权

（一）PC系统信息

edda@EDDA-SH-Linux-01:~$ uname -a
Linux EDDA-SH-Linux-01 5.4.19-rt10 #1 SMP PREEMPT_RT Tue Aug 23 13:57:00 CST 2022 x86_64 x86_64 x86_64 GNU/Linux
edda@EDDA-SH-Linux-01:~$ gcc --version
gcc (Ubuntu 7.5.0-3ubuntu1~18.04) 7.5.0
Copyright (C) 2017 Free Software Foundation, Inc.
This is free software; see the source for copying conditions.  There is NO
warranty; not even for MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

（二）preempt实时包

参考：机械臂官方教程

https://frankaemika.github.io/docs/installation_linux.html#setting-up-the-real-time-kernel

(三)代码

can.cpp

can.hpp

#include <iostream>
#include <thread>
#include "can.hpp"
#include <iso646.h>

extern "C"
{
#include <ECI113.h>
#include <EciDemoCommon.h>
#include <EciCanDemo113.h>
#include <config_usbToCan_ixxat.h>
#include <ctrl_config.h>
#include <OsEci.h>
#include <ECI_pshpack1.h>
#include <string.h>
#include <limits.h>
#include <pthread.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <unistd.h>
}

// gripPara
ECI_CTRL_HDL dwCtrlHandle = ECI_INVALID_HANDLE;
ECI_RESULT hResult = ECI_OK;
DWORD dwHwIndex = 0;
DWORD dwCtrlIndex = 0;
can_TxPDO gripCmd;
can_RxPDO gripState; // gripState
// int gripState = 0;//gripStatue
bool gripStop = false;

#if 0 
int main(int argc, char **argv) //主函数  流程切换控制
{
#if 0
    if (argc != 1)
    {
        std::cout << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
        return -1;
    }
    std::thread thrd_Grip(ThreadGrip);  
     thrd_Grip.detach();
#endif

    // gripCmd.control_word=6;
    ThreadGrip();

    return 0;
}
#endif

void *ThreadGrip(void *data);
int main(int argc, char *argv[])
{
    struct sched_param param;
    pthread_attr_t attr;
    pthread_t thread;
    int ret;

    // 获取核数
    int CPU = 0;
    int CPU_NUM = sysconf(_SC_NPROCESSORS_CONF);
    printf("System has %i processor(s). \n", CPU_NUM);
    cpu_set_t mask; // CPU核的集合

    CPU_ZERO(&mask);
    // set CPU MASK
    CPU_SET(CPU, &mask);

    /* Lock memory */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
    {
        printf("mlockall failed: %m\n");
        exit(-2);
    }

    /* Initialize pthread attributes (default values) */
    ret = pthread_attr_init(&attr);
    if (ret)
    {
        printf("init pthread attributes failed\n");
        goto out;
    }

    /* Set a specific stack size  */
    printf("PTHREAD_STACK_MIN=%d\n", PTHREAD_STACK_MIN); // 16384
    ret = pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN);
    if (ret)
    {
        printf("pthread setstacksize failed\n");
        goto out;
    }

    /* Set scheduler policy and priority of pthread */
    ret = pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    if (ret)
    {
        printf("pthread setschedpolicy failed\n");
        goto out;
    }

    param.sched_priority = 99;
    ret = pthread_attr_setschedparam(&attr, &param);
    if (ret)
    {
        printf("pthread setschedparam failed\n");
        goto out;
    }
    /* Use scheduling parameters of attr */
    ret = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    if (ret)
    {
        printf("pthread setinheritsched failed\n");
        goto out;
    }

    /* Create a pthread with specified attributes */
    ret = pthread_create(&thread, &attr, ThreadGrip, NULL);
    if (ret)
    {
        printf("create pthread failed\n");
        goto out;
    }
#if 1
    //设置当前进程的CPU Affinity

    if (pthread_setaffinity_np(thread, sizeof(mask), &mask) != 0)
    {
        printf("warning: could not set CPU affinity, continuing...\n");
    }
#endif

    /* Join the thread and wait until it is done */
    ret = pthread_join(thread, NULL);
    if (ret)
    {
        printf("join pthread failed: %m\n");
    }

out:
    return ret;
}


void *ThreadGrip(void *data) //  器械控制程序
{
    hResult = init_driver_eci(&dwHwIndex, &dwCtrlIndex, &dwCtrlHandle); // init can
    if (ECI_OK != hResult)
    {
        printf("[ERROR] init_driver ERROR!\n");
    }
    else
    {
        printf("[Note] init_driver  Success!\n");
    }

    hResult = NMTControl(dwCtrlHandle, NMT_Start_Node, 1); // start node
    if (ECI_OK != hResult)
    {
        printf("[ERROR] NMTControl ERROR!\n");
    }
    else
    {
        printf("[Note] NMTControl  Success!\n");
    }
    sleep(1);

    /*==========================================================================*/
    can_RxPDO can_RxPDO_info;
    can_RxPDO_info.state_word = 200; //任意数值
    can_RxPDO_info.act_pos = 200;
    can_RxPDO_info.act_vec = 200;
    can_RxPDO_info.act_torque = 200;
    can_RxPDO_info.act_current = 200;
    can_RxPDO_info.log_input = 200;
    can_RxPDO_info.phy_input = 200;
    can_RxPDO_info.state = 200;

    int cmdState = 0;
    int idx = 0;
    int flag = 0;

    struct timespec ts_1;
    struct timespec ts_2;
    struct timespec ts_3;
    struct timespec ts_4;
    long delta34 = 0;

    clock_gettime(CLOCK_MONOTONIC, &ts_1);
    clock_gettime(CLOCK_MONOTONIC, &ts_2);
    clock_gettime(CLOCK_MONOTONIC, &ts_3);
    clock_gettime(CLOCK_MONOTONIC, &ts_4);

    // printf("CLOCK_REALTIME   1: %d, %d\n", ts_1.tv_sec, ts_1.tv_nsec);
    // printf("CLOCK_REALTIME   2: %d, %d\n", ts_2.tv_sec, ts_2.tv_nsec);

    while (!gripStop)
    {
        // printf("idx=%d\n", idx);
        idx++;
        int a = receivePDO(dwCtrlHandle, &can_RxPDO_info); //接受PDO

        switch (cmdState)
        {
        case SwitchOnDisabled: // switch on disabled 初始化完成，可以进行CAN通讯（WP）
        {
            // printf("[SwitchOnDisabled] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);

            if ((can_RxPDO_info.state_word & 0x0040) == 0x0040) //  0040对应的状态字是0100 0000，SwitchOnDisabled （WP）
            {
                gripCmd.control_word = 0x0006; // 06对应的含义是从初始化这一步进到下面ReadyToSwitchOn状态的控制字（WP）
                cmdState = ReadyToSwitchOn;
            }

            break;
        }

        case ReadyToSwitchOn: //该状态等待进入SwitchOn状态，此时电机没有被励磁(WP)
        {
            // printf("[ReadyToSwitchOn] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[ReadyToSwitchOn] act_pos=%d\n", can_RxPDO_info.act_pos);

            gripCmd.mode = 06; //选择运行模式为归零模式(WP)
            gripCmd.home_position = 0;
            gripCmd.homing_method = 37;                //-3 37
            gripCmd.current_threshold_of_homing = 100; // max current threshold
            gripCmd.homing_speed_for_search = 100;

            if ((can_RxPDO_info.state_word & 0x0021) == 0x0021) //  0021对应的状态字是0010 0001，就是ReadyToSwitchOn （WP）
            {
                gripCmd.control_word = 0x0007; // 07对应的含义是由等待SwitchOn状态进入到StwitchedOn状态的控制字（WP）
                cmdState = SwitchedOn;
            }
            break;
        }


       case SwitchedOn: //该状态主电已上，输入励磁信号并开始控制电机（WP）
        {
            // printf("[SwitchedOn] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[SwitchedOn] act_pos=%d\n", can_RxPDO_info.act_pos);
            // printf("[SwitchedOn] mode_display=0x%x\n", can_RxPDO_info.mode_display);

            if ((can_RxPDO_info.state_word & 0x0023) == 0x0023) // 0023对应的状态字是0010 0011，就是SwitchedOn状态（WP）
            {
                gripCmd.control_word = 0x000F; //主电上成功后，接下来进入使能状态的控制字就是0F（也就是15）（WP）
                cmdState = OperationEnabled;
            }
            break;
        }

        case OperationEnabled:
        {
            // printf("[OperationEnabled] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[OpeartionEnabled] act_pos=%d\n", can_RxPDO_info.act_pos);

            if ((can_RxPDO_info.state_word & 0x0027) == 0x0027) // 0027对应的状态字是0010 0111，就是OperationEnabled状态（WP）
            {
                cmdState = ServoSetHoming;
            }
            break;
        }

        case ServoSetHoming:
        {
            // printf("[ServoSetHoming] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[ServoSetHoming] act_pos=%d\n", can_RxPDO_info.act_pos);

            //  gripCmd.mode = 06; //选择运行模式为归零模式(WP)
            //  gripCmd.home_position = 0;
            //  gripCmd.homing_method = -3; //-3 37
            //  gripCmd.current_threshold_of_homing = 120;
            //  gripCmd.homing_speed_for_search = 100;

            if (can_RxPDO_info.mode_display == 06)
            {
                gripCmd.control_word = 0x001F; //控制字运行归零模式 0001 1111 (WP)
                cmdState = WaitingHoming;
            }
        }

       case WaitingHoming:
        {
            // printf("[WaitingHoming] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[WaitingHoming] act_pos=%d\n", can_RxPDO_info.act_pos);

            if ((can_RxPDO_info.state_word & 0x9000) == 0x9000) // 00x9000
            {
                printf(" ********** Arrive Home **********\n");
                //usleep(10000);
                // cmdState = 10;
                cmdState = ServoModeHomingEnable;

            }
            break;
        }

        case ServoModeHomingEnable:
        {

            // printf("[ServoModeHomingEnable] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[ServoModeHomingEnable] act_pos=%d\n", can_RxPDO_info.act_pos);

            // if ((can_RxPDO_info.state_word & 0x0014) == 0x0014) // 0024对应的状态字是0001 0100，operation enabled and voltage enabled (WP)
            // {
            gripCmd.target_pos = can_RxPDO_info.act_pos;
            gripCmd.mode = 8;               // set csp
            gripCmd.interpolation_time = 2; // ms 插补时间
            cmdState = ServoModeCycPosition;
            idx = 0;

            break;
        }

        case ServoModeCycPosition:
        {
            // printf("[ServoModeCycPosition] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[INFO START]flag=%d act_vec=%d\n", flag, can_RxPDO_info.act_vec);
            printf("[ServoModeCycPosition]can_RxPDO_info.act_pos=%d  \n", can_RxPDO_info.act_pos);
            printf("[ServoModeCycPosition]gripCmd.target_pos====%d  \n", 300*flag);
            printf("[ServoModeCycPosition]delta==============%d  \n", 300*flag-can_RxPDO_info.act_pos);

            // gripCmd.target_vec = 1000;

            // printf("[INFO START]flag=%d act_pos=%d\n", flag, can_RxPDO_info.act_pos);


#if 1
            if (flag < 1000)
            {
                gripCmd.target_pos +=300;
            }
            // else if (flag >= 4000 && flag < 8000)
            // { 
            // gripCmd.target_pos -= 100;
            //}
            else
            {
                cmdState = 10;
            }

#endif
            // printf("[INFO END]flag=%d act_vec=%d\n", flag, can_RxPDO_info.act_vec);
            // printf("[INFO END]flag=%d act_pos=%d\n", flag, can_RxPDO_info.act_pos);

            flag = flag + 1;

            break;
        }

        case 10:
        {
            // printf("[10] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[10] act_pos=%d\n", can_RxPDO_info.act_pos);
            gripStop = 1;

            break;
        }
        case 20:
        {
            // printf("[20] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);

            // printf("[20] act_pos=%d\n", can_RxPDO_info.act_pos);

            cmdState = 30;
        }

        case 30:
        {
            // printf("[30] can_RxPDO_info.state_word=0x%x\n", can_RxPDO_info.state_word);
            // printf("[30] act_pos=%d\n", can_RxPDO_info.act_pos);

            gripStop = 1;
            break;
        }

        default:
            break;
        } // switch



        hResult = sendPDO(dwCtrlHandle, gripCmd, &ts_1, &ts_2, &ts_3, &ts_4, &delta34); //发送PDO
                                                                                        // usleep(2000);
    }                                                                                   // for-----while

    sleep(1);

    hResult = release_driver_eci(&dwHwIndex, &dwCtrlIndex, &dwCtrlHandle); // stop can

can.hpp

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <array>
#include <iostream>
extern "C"
{
#include <ECI113.h>
#include <EciDemoCommon.h>
#include <EciCanDemo113.h>
#include <config_usbToCan_ixxat.h>
#include <ctrl_config.h>
#include <OsEci.h>
#include <ECI_pshpack1.h>
#include <string.h>
}
#define USEC_PER_SEC 1000000
#define NSEC_PER_SEC 1000000000

namespace
{
// can
#define SwitchOnDisabled 0x00
#define ReadyToSwitchOn 0x06
#define SwitchedOn 0x07
#define OperationEnabled 0x0F
#define ServoCmdDisable 0x09 // 0x07

#define ServoSetHoming 0x16 //回零模式
#define ServoModeHomingEnable 0x26
#define ServoModeHoming 0x06      //回零模式
#define ServoModeCycPosition 0x08 // 周期位置模式
#define ServoModeCycTorque 0x0A   //周期力矩模式
#define WaitingHoming 0x1000      //等待模式

#define omegaL_gripDeg_close -0.012
#define omegaL_gripDeg_open -28.4518

#define omegaR_gripDeg_close 0.012
#define omegaR_gripDeg_open 28.4518

#define HomingModeNegative -3           //正向回零
#define HomingModeNegativeToPositive -1 //正向回零再反转
#define HomingModePositive -4           //反向回零
#define HomingModePositiveToNegative -2 //负向回零再正转

#define NMT_Start_Node 0x01
#define NMT_Stop_Node 0x02
#define NMT_Enter_PreOperational 0x80
#define NMT_Reset_Node 0x81
#define NMT_Reset_Comunication 0x82


  typedef struct CAN_TxPDO
  {
    uint16_t control_word;
    int32_t target_pos;
    int32_t target_vec;
    int32_t home_position;
    int16_t target_torque;
    int8_t homing_method;
    int8_t mode;
    int8_t state;
    int16_t current_threshold_of_homing;
    int32_t homing_speed_for_search;
    uint8_t interpolation_time;
  } can_TxPDO;
  typedef struct CAN_RxPDO
  {
    uint16_t state_word;
    int8_t mode_display;
    int32_t act_pos;
    int32_t act_vec;
    int16_t act_torque;
    int16_t act_current;
    uint8_t log_input;
    uint8_t phy_input;
    int8_t state;
  } can_RxPDO;
  // can

  static long cycs = 0;
  static inline void tsnorm(struct timespec *ts)
  {
    while (ts->tv_nsec >= NSEC_PER_SEC)
    {
      ts->tv_nsec -= NSEC_PER_SEC;
      ts->tv_sec++;
    }
  }
  ECI_RESULT NMTControl(ECI_CTRL_HDL dwCtrlHandle, BYTE NMT_CMD, BYTE Node_Id)
  {
    ECI_RESULT hResult = ECI_OK;
    ECI_CTRL_MESSAGE stcCtrlMsg = {0};
    //*** Prepare CAN Message to send
    stcCtrlMsg.wCtrlClass = ECI_CTRL_CAN;
    stcCtrlMsg.u.sCanMessage.dwVer = ECI_STRUCT_VERSION_V0;
    stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x000;
    stcCtrlMsg.u.sCanMessage.u.V0.uMsgInfo.Bits.dlc = 2;
    stcCtrlMsg.u.sCanMessage.u.V0.abData[0] = NMT_CMD;
    stcCtrlMsg.u.sCanMessage.u.V0.abData[1] = Node_Id;
    OS_Sleep(1);
    //*** Send one message
    hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);
    OS_Sleep(10);
   /***************************************************/
    if (ECI_OK != hResult)
    {
      OS_Printf("Error while sending CAN Messages\n");
      ECIDEMO_CHECKERROR(ECI113_CtrlSend);
    }
    return hResult;
  }

  int receivePDO(ECI_CTRL_HDL dwCtrlHandle, can_RxPDO *can_RxPDO_info)
  {
    // printf("file=%s func=%s line=%d\n", __FILE__, __FUNCTION__, __LINE__);

#if 0
    can_RxPDO_info->state_word = 0;
    can_RxPDO_info->act_pos = 0;
    can_RxPDO_info->act_vec = 0;
    can_RxPDO_info->act_torque = 0;
    can_RxPDO_info->act_current = 0;
    can_RxPDO_info->log_input = 0;
    can_RxPDO_info->phy_input = 0;
    can_RxPDO_info->state = 0;
#endif

    ECI_CTRL_MESSAGE astcCtrlMsg[20] = {{0}};
    DWORD dwCount = _countof(astcCtrlMsg);
    DWORD dwMsgIndex = 0;
    ECI_RESULT hResult = ECI_OK;
    //*** Try to read some message

    hResult = ECI113_CtrlReceive(dwCtrlHandle, &dwCount, astcCtrlMsg, ECIDEMO_RX_TIMEOUT);

    if (ECI_OK != hResult)
    {
      printf("[reseive_PDO_ERROR]\n");
      ECIDEMO_CHECKERROR(ECI113_CtrlReceive);
      printf("\n");
      can_RxPDO_info->state = 1;
      return 0;
    }
    while ((ECI_OK == hResult) && (dwCount > dwMsgIndex))
    {
      ECI_CTRL_MESSAGE *pstcCtrlMsg = &astcCtrlMsg[dwMsgIndex];
      DWORD b1 = pstcCtrlMsg->u.sCanMessage.u.V0.dwMsgId;
      uint8_t *abData = pstcCtrlMsg->u.sCanMessage.u.V0.abData;
      // printf("[INFO]dwMsgIndex=%d COBID=0x%x\n",dwMsgIndex, b1);

      // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
      // OS_Printf("\n");


      // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
      // OS_Printf("\n");
      int nodeID = 0x01;
      if (b1 == 0x180 + nodeID)
      {
        memcpy(&can_RxPDO_info->state_word, &abData[0], 2);
        memcpy(&can_RxPDO_info->mode_display, &abData[2], 4);

        // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
        // OS_Printf("\n");
      }
      else if (b1 == 0x280 + nodeID)
      {
        memcpy(&can_RxPDO_info->state_word, &abData[0], 2);
        memcpy(&can_RxPDO_info->act_pos, &abData[2], 4);
        // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
        // OS_Printf("\n");
        // printf("act_pos=%d\n",can_RxPDO_info->act_pos);
      }
      else if (b1 == 0x380 + nodeID)

      {
        memcpy(&can_RxPDO_info->state_word, &abData[0], 2);
        memcpy(&can_RxPDO_info->act_vec, &abData[2], 4);
        // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
        //  OS_Printf("\n");
      }
      else if (b1 == 0x480 + nodeID)
      {
        memcpy(&can_RxPDO_info->state_word, &abData[0], 2);
        memcpy(&can_RxPDO_info->act_torque, &abData[2], 2);
        memcpy(&can_RxPDO_info->act_current, &abData[4], 2); //
        //    EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
        //   OS_Printf("\n");
        //   printf("state_word=%d  act_torque=%d  act_current=%d  \n",can_RxPDO_info->state_word,can_RxPDO_info->act_torque,can_RxPDO_info->act_current);
      }
      else if ((b1 != 0x180 + nodeID) && (b1 != 0x280 + nodeID) && (b1 != 0x380 + nodeID) && (b1 != 0x480 + nodeID))
      {
        printf("[reseive_PDO_ERROR]: receiveMessage failed!");
        EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
        OS_Printf("\n");
        can_RxPDO_info->state = 2;
      }
      //*** Proceed with next message
      dwMsgIndex++;
    } // end while
    //*** Reset Error code and proceed with transmission
    hResult = ECI_OK;
    return 0;
  } //主函数


  HRESULT sendPDO(ECI_CTRL_HDL dwCtrlHandle, can_TxPDO canTxPDO, struct timespec *ts_1, struct timespec *ts_2, struct timespec *ts_3, struct timespec *ts_4, long *delta34)
  {
    // printf("file=%s func=%s line=%d\n", __FILE__, __FUNCTION__, __LINE__);

    ECI_RESULT hResult = ECI_OK;
    /***********************************************************************/
    //*** Send some CAN Messages
    ECI_CTRL_MESSAGE stcCtrlMsg = {0};
    DWORD dwTxMsgCount = 5; // ECIDEMO_TX_MSGCOUNT_CAN;
    DWORD dwIndex = 0;
    //*** Send Loop
    for (dwIndex = 0; dwIndex < dwTxMsgCount; dwIndex++)
    {
      //*** Prepare CAN Message to send
      stcCtrlMsg.wCtrlClass = ECI_CTRL_CAN;
      stcCtrlMsg.u.sCanMessage.dwVer = ECI_STRUCT_VERSION_V0;
      stcCtrlMsg.u.sCanMessage.u.V0.uMsgInfo.Bits.dlc = 8;
      uint8_t *abData = stcCtrlMsg.u.sCanMessage.u.V0.abData;
      // OS_Sleep(200);
      int nodeID = 0x01;
      if (dwIndex == 0)
      {
        stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x200 + nodeID;
        memcpy(&abData[0], &(canTxPDO.control_word), 2);
        memcpy(&abData[2], &(canTxPDO.mode), 1);
        memcpy(&abData[3], &(canTxPDO.homing_method), 1);
        // mempcp(&abData[4],&(canTxPDO.home_position),4);
      }
      if (dwIndex == 1)
      {
        stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x300 + nodeID;
        memcpy(&(abData[0]), &(canTxPDO.control_word), 2);
        memcpy(&(abData[2]), &(canTxPDO.target_pos), 4);
        memcpy(&(abData[6]), &(canTxPDO.current_threshold_of_homing), 2);
      }
      if (dwIndex == 2)
      {
        stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x400 + nodeID;
        memcpy(&(abData[0]), &(canTxPDO.control_word), 2);
        memcpy(&(abData[2]), &(canTxPDO.target_vec), 4);
        memcpy(&(abData[6]), &(canTxPDO.interpolation_time), 1);
      }

      if (dwIndex == 3)
      {
        stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x500 + nodeID;
        memcpy(&(abData[0]), &(canTxPDO.control_word), 2);
        memcpy(&(abData[2]), &(canTxPDO.target_torque), 2);
        memcpy(&(abData[4]), &(canTxPDO.homing_speed_for_search), 4);
      }


      if (dwIndex == 4)
      {
        stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x080;
        stcCtrlMsg.u.sCanMessage.u.V0.uMsgInfo.Bits.dlc = 0;

//plan A
#if 1
        long time = 2000000; // 5000000ns=5ms

        long delta_nsec,  delta_sec;
        long remain_ns = 0;

        /* Get current time */
        clock_gettime(CLOCK_MONOTONIC, ts_2);

        delta_sec = ts_2->tv_sec - ts_1->tv_sec;
        delta_nsec = delta_sec * 1000000000 + ts_2->tv_nsec - ts_1->tv_nsec; //+ *delta34;
        remain_ns = time - delta_nsec > 10000  ? time - delta_nsec : 10000;


        //printf("used_ns=%lu  remain_ns=%lu\n", delta_nsec, remain_ns);


        ts_2->tv_nsec += remain_ns;
        tsnorm(ts_2);
        ts_1->tv_sec = ts_2->tv_sec;
        ts_1->tv_nsec = ts_2->tv_nsec;

        int ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, ts_2, NULL);
        hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);
#endif



//plan B
#if 0
        ts_2->tv_nsec += 1950000;
        tsnorm(ts_2);

        int ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, ts_2, NULL);
        hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);
#endif




        break;
      } // if (dwIndex == 4)

      //*** Wait some time to ensure bus idle
      // OS_Sleep(1);
      //*** Send one message
      hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);
      if (ECI_OK != hResult)
      {
        printf("[PDOSendERROR]: /n");
        ECIDEMO_CHECKERROR(ECI113_CtrlSend);
        printf("/n");
      }
    } // end for for (dwIndex = 0; dwIndex < dwTxMsgCount; dwIndex++)

    return hResult;
  }

  DWORD ReadSDO(ECI_CTRL_HDL dwCtrlHandle, WORD index, BYTE Subindex, BYTE dCount)
  {
    ECI_RESULT hResult = ECI_OK;
    ECI_CTRL_MESSAGE stcCtrlMsg = {0};
    DWORD SDO_info;
    stcCtrlMsg.wCtrlClass = ECI_CTRL_CAN;
    stcCtrlMsg.u.sCanMessage.dwVer = ECI_STRUCT_VERSION_V0;
    stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x601;
    stcCtrlMsg.u.sCanMessage.u.V0.uMsgInfo.Bits.dlc = 8;
    uint8_t *abData = stcCtrlMsg.u.sCanMessage.u.V0.abData;
    abData[0] = 0x40;
    memcpy(&abData[1], &(index), 2);
    memcpy(&abData[3], &(Subindex), 1);
    abData[4] = 0;
    abData[5] = 0;
    abData[6] = 0;
    abData[7] = 0;
    hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);
    if (ECI_OK != hResult)
    {
      printf("[PDOSendERROR]: \n");
      ECIDEMO_CHECKERROR(ECI113_CtrlSend);
      printf("/n");
      return 0;
    }
    OS_Sleep(5);

    ECI_CTRL_MESSAGE astcCtrlMsg[20] = {{0}};
    DWORD dwCount = _countof(astcCtrlMsg);
    hResult = ECI113_CtrlReceive(dwCtrlHandle, &dwCount, astcCtrlMsg, ECIDEMO_RX_TIMEOUT);
    if (ECI_OK != hResult)
    {
      printf("[PDOSendERROR]: \n");
      ECIDEMO_CHECKERROR(ECI113_CtrlReceive);
      printf("/n");
      return 0;
    }
    else
    {
      ECI_CTRL_MESSAGE *pstcCtrlMsg = &astcCtrlMsg[0];
      DWORD cobID = pstcCtrlMsg->u.sCanMessage.u.V0.dwMsgId;
      uint8_t *SDOData = pstcCtrlMsg->u.sCanMessage.u.V0.abData;

      // EciPrintCtrlMessage(&astcCtrlMsg[dwMsgIndex]);
      // OS_Printf("\n");
      if (cobID == 0x581)
      {
        memcpy(&SDO_info, &abData[4], 4);
        return SDO_info;
      }
    }
    return SDO_info;
  }

  ECI_RESULT WriteSDO(ECI_CTRL_HDL dwCtrlHandle, BYTE index, BYTE Subindex, BYTE length, DWORD data)
  {
    ECI_RESULT hResult = ECI_OK;

    ECI_CTRL_MESSAGE stcCtrlMsg = {0};
    stcCtrlMsg.wCtrlClass = ECI_CTRL_CAN;
    stcCtrlMsg.u.sCanMessage.dwVer = ECI_STRUCT_VERSION_V0;
    stcCtrlMsg.u.sCanMessage.u.V0.dwMsgId = 0x601;
    stcCtrlMsg.u.sCanMessage.u.V0.uMsgInfo.Bits.dlc = 8;
    uint8_t *abData = stcCtrlMsg.u.sCanMessage.u.V0.abData;
    abData[0] = length;
    memcpy(&abData[1], &(index), 2);
    memcpy(&abData[3], &(Subindex), 1);
    if (length == 0x2F)
    {
      memcpy(&abData[4], &(data), 1);
    }
    if (length == 0x2B)
    {
      memcpy(&abData[4], &(data), 2);
    }
    if (length == 0x27)
    {
      memcpy(&abData[4], &(data), 3);
    }
    if (length == 0x23)
    {
      memcpy(&abData[4], &(data), 4);
    }
    hResult = ECI113_CtrlSend(dwCtrlHandle, &stcCtrlMsg, ECIDEMO_TX_TIMEOUT);

    if (ECI_OK != hResult)
    {
      printf("[PDOSendERROR]:\n");
      ECIDEMO_CHECKERROR(ECI113_CtrlSend);
      printf("/n");
      return 0;
    }
    return 0;
  }

} // namespace name

吃我老猪一钉耙
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
实时Linux之PREEMPT_RT篇
Alan何佛爷的博客
07-18 1万+
实时Linux—PREEMPT_RT篇 实时Linux主要有两类方案： 单内核方案：对主线传统的Linux内核打入PREEMPT_RT补丁，使内核成为硬实时操作系统 双内核方案：主线传统Linux内核+实时内核的双内核方案，常见的主流方式有：RT-Linux，RTAI、Xenomai。 本文重点讲解单内核PREEMPT_RT补丁方案。 概念基础 实时性 ​ 实时分为硬实时和软实时，两者主要的区别主要就是就绪运行时间确定性。然后，主线Linux是软实时系统，加入实时补丁后将其改造为硬实时系统。 可抢占性
实时Linux内核（PREEMPT_RT）的编译安装以及测试
热门推荐
闲人的专栏
07-06 3万+
​ ​​​​​​​1.什么是实时性操作系统？ https://blog.csdn.net/ywx123_/article/details/53861274 实时性是指调度的时候，任务响应时间。windows一般是15ms，最大的问题是不能保证。比如平均值是1ms，但是随着系统负载的变化，有时甚至达到100ms，在这工业上是无法使用的。工业上一些应用要求必须有更高的时间精度，比如，一个电力监测系统必须在10ms内运行一次任务对电力运行状况进行监测，一旦时间不准，调度不到该程序运行，则无法保证对电力故障的及时
内核模块常用操作命令
zcx_5517的博客
06-04 259
4. depend modprobe是怎么知道一个给定模块所依赖的其他的模块呢？在这个过程中，depend起到了决定性作用，当执行modprobe时， 它会在模块的安装目录下搜索module.dep文件，这是depmod创建的模块依赖关系的文件。3. modprobe 与insmod类似，除此以外modprobe还能检查模块之间的依赖关系， 并且按照顺序加载这些依赖，可以理解为按照顺序多次执行insmod。rmmod不会卸载一个模块所依赖的模块，需要依次卸载，当然是用modprobe -r 可以一键卸载。
嵌入式驱动模块的加载与卸载
新时代农民工---码农
04-19 3264
最近学习正点原子嵌入式驱动模块加载与卸载的时候，使用命令：modprobe chrdevbase.ko 出现 hrdevbase: version magic '4.1.15 SMP preempt mod_unload modversions ARMv6 p2v8 ’ should be '4.1.15 SMP preempt mod_unload modversions ARMv7 p2v8 ’ 问题原因：这种报错是内核版本和模块版本不一致，所以导致加载不成功 温馨提示：这里的内核版本不是ubun
关于CONFIG_LOCALVERSION_AUTO设置去掉内核版本号SVN后缀
GQB的专栏
12-27 7964
原创作品，转载时请务必以超链接形式标明文章原始出处:http://blog.csdn.net/gqb666/article/details/8444528,作者:gqb666 最近在TI 的DVSDK下写驱动模块时老受linux内核svn版本号问题的困扰，如"2.6.37-svn41"、"2.6.37-svn51"等等，svn版本变一次，从上面取下的代码内核版本就要变一次，这样造成原来驱动模
树莓派4B实时内核（Preempt_RT)安装教程，实时系统安装教程，实时内核编译，实时系统编译，Preempt编译
03-31
树莓派4B实时内核（Preempt_RT）安装教程主要涵盖了如何在树莓派4B上构建和安装一个实时操作系统。实时系统是那种能够快速响应外部事件的系统，尤其适用于需要精确时间控制的应用，如自动化、机器人或者音视频处理等...
RT-Tools-RPi：使用RT-Preempt修补Rasbian内核的工具
02-05
本文将深入探讨RT-Tools-RPi工具，它是针对Raspberry Pi进行RT-Preempt内核修补的一个实用工具集。 **RT-Preempt简介** RT-Preempt是Linux内核的一个补丁集，旨在增强其实时性能。它通过减少上下文切换的时间和提高...
Linux 内核实时补丁
11-23
Linux内核实时补丁，尤其是PREEMPT_RT补丁，是针对标准Linux内核进行的增强，以提高其处理硬实时任务的能力。标准Linux内核虽然可以满足软中断的需求，并提供基本的Posix操作，但在时间响应方面并不保证确定性，这...
Preempt_rt实时补丁安装指南
12-17
总结，安装Preempt_RT实时补丁需要准确无误地执行每一步，尤其在配置内核阶段，确保实时特性的启用是成功的关键。通过遵循这个指南，你将能够提升你的Linux系统实时性能，使其更加适合那些对响应时间有苛刻要求的...
Linux内核情景分析
11-14
1. **模块化设计**：Linux内核采用模块化设计，允许根据需要加载或卸载特定的硬件驱动，这使得内核更加灵活，能适应不同的硬件配置。 2. **调度算法**：Linux内核使用了多种调度算法，如CFS（ Completely Fair ...
linux内核与rt-preempt补丁
07-01
提供了目前最新的赛灵思zynq的arm-linux内核，可支持ZC702开发板。内核版本为4.14，并提供了相对应的rt-preempt补丁，通过patch命令可让标志的linux软实时变为硬实时，通过实践代码能顺利编译，并完美生成uImage和devicetree.dts。
内核模块--module学习
qq_33789039的博客
09-13 710
一、内核模块
关于linux动态库中无法设置线程栈大小的问题
cjsycyl的专栏
08-31 1382
前两天同事遇到一个问题，在写一款基于嵌入式Linux的web服务器的时候需对服务线程的栈大小进行设置，在静态库和demon程序里使用pthread_attr_setstacksize(&attr,STACK_SIZE)进行线程栈大小设置的时候都没问题，可一旦在动态库中进行设置的时候就没效果了，函数返回正常，就是线程的栈大小在内存映射工具中显示不对。 折腾了好久，最后发现原来是生成动态库的命令中没
使用linux c开源库libwebsockets编写的websocket客户端
c_cppcoder的博客
02-11 7540
<一>: 背景 19年中旬做的一个嵌入式项目, 应用层需要有一个心跳的功能,当时决定用websocket协议, 所以当时就研究了下libwebsockets的使用. 网上的资料并不多, 当时只能研究开源库提供的测试代码, 然后改改后就合并到项目里了. 这几天又把libwebsockets重新编译了下, 顺手重新写了一个简单的websocket客户端. 主要包含了以下三个功能: 1...
linux实时化改造：Preempt-rt 和 Xenomai
weixin_39917818的博客
08-19 3772
linux实时化改造：RT-Preempt 和 XenomaiXenomai三级目录 Xenomai <1> Xenomai简介-百度百科 关键词： 双内核，在linux内核之外构造一个实时微内核。 微内核负责处理系统的实时任务，而Linux 则负责处理非实时任务，只有当实时内核不再有实时任务需要处理的时候，Linux 内核才能得到运行的机会 <2> Xenomai实现原理 - （Adeos） 三级目录 ...
树莓派4B实时内核（Preempt_RT)安装教程，实时系统安装教程，实时内核编译，实时系统编译，Preempt编译，Preempt安装，树莓派实时系统，树莓派实时内核
benandpao的专栏
03-31 2247
本文记录了树莓派4B编译安装Preempt实时内核详细的教程，编译过程的指令及指令输入的图片、配置过程图片全部进行了截图总结，方便大家参考。 编译于2022年3月31日上午测试成功，并制作了RT系统的镜像文件。不想编译的，可联系我下载编译成功的镜像文件（8GB）。
树莓派 raspberrypi 6.1（无桌面版本）固定有线ip地址
最新发布
yhbstf的博客
01-23 853
随着 Raspberry Pi OS Bookworm 的发布，Raspberry Pi 上的网络改为使用 NetworkManager 作为网络的标准控制器，取代了之前的 dhcpcd 系统。NetworkManager 包括一个名为“nmcli”的命令行工具，它可以控制 NetworkManager 并报告网络状态。我们将为 Raspberry Pi 指定一个静态 IP 地址 192.168.2.2。本教程将使用“nmcli”将网络配置为使用静态 IP 地址。
manjaro 搜狗输入法_Manjaro日常使用 之一：日常办公
weixin_39626237的博客
11-28 203
系统信息下：Linux ｗ-pc 4.19.80-1-MANJARO #1 SMP PREEMPT Fri Oct 18 12:28:02 UTC 2019 x86_64 GNU/Linux桌面环境：Xfce4.14已安装软件：wps-office 微信　腾讯TIM 搜过输入法　VLC LibreOffice　360安全浏览器国产版等日常办公常用软件使用时间：2019年８月－至今相关图示：安装过程...
GCD全解-dispatch_after/dispatch_time-t延迟操作
chenzimin_blog
02-20 1892
1、延后执行：dispatch_after 2、延长时间：dispatch_time_t 3、拓展
"Fully Preemptible Kernel (RT)内核配置中没有这个选项怎么办
05-13
如果您的Linux内核版本不支持Fully Preemptible Kernel (RT)选项，那么您需要升级您的内核版本。 请注意，Fully Preemptible Kernel (RT)选项是在Linux 2.6.33版本中引入的，因此如果您的内核版本比这个更旧，那么您需要首先升级到此版本或更高版本。 如果您已经使用了支持Fully Preemptible Kernel (RT)选项的内核版本，但是该选项仍然不可用，那么您需要检查您的内核配置是否正确。请确保已经启用了CONFIG_PREEMPT和CONFIG_PREEMPT_RT两个选项，并且已经禁用了CONFIG_PREEMPT_VOLUNTARY选项。 如果您仍然无法找到Fully Preemptible Kernel (RT)选项，请考虑使用其他的实时内核，例如Xenomai或RTAI。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值