主页 / 问题 / 1356094
Accepted
Anton-Vas
Asked:2022-05-01 21:27:05 +0000 UTC2022-05-01 21:27:05 +0000 UTC

STM32 不接受通过 UART 的 MAVLink 命令

  • 772

来自一篇关于将自动驾驶仪连接到机上附加计算机的精彩文章 ( https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566 ) 的源代码。

我为我的任务重做了它，但原理保持不变。与源的唯一区别是不同的微控制器。我在 Arduino MINI 上进行了测试，但对于未来的升级来说还不够。因此，我将代码转移到 STM32 轨道上，复杂性就出现了。

- - - - - - - - - - - - - - - - - - - - - - - 问题 - - --------------------------------------------------

  • 在下面的代码中，不接受请求的命令！

我尽量不改变文章作者的方法，但没有直接模拟 Arduino HAL 函数：available()。所以我尝试用更合适的 HAL_UART_GetState() 替换它。

  • 目前尚不清楚问题是什么：while（...）条件或几个HAL_UART_Receive_IT / _Transmit的冲突

用 STM32CubeIDE 编写，MAVLink 库来自文章，STM32F411CEU6 微控制器，通过 Termite 上的 UART (FTDI Arduino) 输出消息。

#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mavlink.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
    ///< Temporary here:
#define MAV_TYPE_GIMBAL 26
#define MAV_COMP_ID_GIMBAL 154

    ///< Debug:
#define DEBUG_MODE
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;

/* USER CODE BEGIN PV */
unsigned long previousMillisMAVLink   = 0;                       ///< will store last time MAVLink was transmitted and listened
unsigned long nextIntervalMAVLink     = 1000;                    ///< next interval to count
const int     c_num_hbs               = 60;                      ///< № of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int           num_hbs_pasados         = c_num_hbs;

    /*MAVLink variables*/
int           sysid                   = 1;                       ///< ID 1: id of plane (stm32 is planes` component)
int           compid                  = MAV_COMP_ID_GIMBAL;      ///< The component sending the message
int           type                    = MAV_TYPE_GIMBAL;         ///< Gimbal

    /*Define the system type, in this case an airplane -> on-board controller*/
uint8_t       system_type             = MAV_TYPE_GENERIC;
uint8_t       autopilot_type          = MAV_AUTOPILOT_INVALID;
uint8_t       system_mode             = MAV_MODE_PREFLIGHT;      ///< Booting up
uint8_t       system_state            = MAV_STATE_STANDBY;       ///< System ready for flight
uint8_t       req_stream_id           = MAV_DATA_STREAM_ALL;
uint8_t       target_system           = 1;                       ///< Id # of Pixhawk (should be 1)
uint8_t       target_component        = 0;                       ///< Target component, 0 = all (seems to work with 0 or 1
uint8_t       start_stop              = 1;                       ///< 1 = start, 0 = stop
uint16_t      req_message_rate        = 0x01;                    ///< number of times per second to request the data in hex
uint32_t      custom_mode             = 0;                       ///< Custom mode, can be defined by user/adopter
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
/* USER CODE BEGIN PFP */
void debugPrint(UART_HandleTypeDef *huart, char _out[]);
void debugPrintln(UART_HandleTypeDef *huart, char _out[]);
void MavLink_Receive();
void Request_Datastream();
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  /* USER CODE BEGIN 2 */
#ifdef DEBUG_MODE
  debugPrintln(&huart1, " STM32F411CEU6.........ACTIVE");
#endif

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
      mavlink_message_t msg;
      uint8_t buf[MAVLINK_MAX_PACKET_LEN];

         /*Pack the message*/
      mavlink_msg_heartbeat_pack( sysid, compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);

         /*Copy the message to the send buffer*/
      uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

      unsigned long currentMillisMAVLink = HAL_GetTick();

      if (currentMillisMAVLink - previousMillisMAVLink >= nextIntervalMAVLink) {
              /* Save the last time you changed the mode */
          previousMillisMAVLink = currentMillisMAVLink;

          HAL_UART_Transmit(&huart2, (uint8_t*)buf, len, 100);

#ifdef DEBUG_MODE
          if(sizeof(buf)){
              debugPrintln(&huart1, " HB......................SENT");
          }
#endif

          num_hbs_pasados++;
          if(num_hbs_pasados>=c_num_hbs) {
              Request_Datastream();
              num_hbs_pasados=0;
          }
      }

      MavLink_Receive();

    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 12;
  RCC_OscInitStruct.PLL.PLLN = 96;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief USART1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 57600;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 57600;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

}

/* USER CODE BEGIN 4 */
void debugPrint(UART_HandleTypeDef *huart, char _out[]){
    HAL_UART_Transmit(huart, (uint8_t *) _out, strlen(_out), 100);
//  HAL_UART_Transmit_IT(huart, (uint8_t *) _out, strlen(_out));
}

void debugPrintln(UART_HandleTypeDef *huart, char _out[]){
    HAL_UART_Transmit(huart, (uint8_t *) _out, strlen(_out), 100);
//  HAL_UART_Transmit_IT(huart, (uint8_t *) _out, strlen(_out));
    char newline[2] = "\r\n";
    HAL_UART_Transmit(huart, (uint8_t *) newline, 2, 100);
//  HAL_UART_Transmit_IT(huart, (uint8_t *) newline, 2);
}

void MavLink_Receive(){

    mavlink_message_t msg;
    mavlink_status_t status;

    while(HAL_UART_GetState(&huart2)==HAL_UART_STATE_READY){           //
                                                                       //
        uint8_t c;                                                     //.......?
//      HAL_UART_Receive(&huart2, &c, 1, 100);                         //
        HAL_UART_Receive_IT(&huart2, &c, 1);                           //

        if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {

#ifdef DEBUG_MODE
            debugPrint(&huart1, " MSG ID: ");
            debugPrintln(&huart2, msg.msgid);
#endif

            switch(msg.msgid){
                case MAVLINK_MSG_ID_COMMAND_LONG:{

#ifdef DEBUG_MODE
                    debugPrintln(&huart1, " >>>Income_MSG.....COMMAND_LONG");
#endif
                    /*Send confirmation to GCS through PX4*/
                    mavlink_message_t resMsg;
                    uint8_t resBuf[MAVLINK_MAX_PACKET_LEN];
                    mavlink_msg_command_ack_pack(sysid, compid, &resMsg, MAV_CMD_DO_MOUNT_CONTROL, MAV_RESULT_ACCEPTED);
                    uint16_t resLen = mavlink_msg_to_send_buffer(resBuf, &resMsg);

#ifdef DEBUG_MODE
                    debugPrint(&huart1, "..........MSG ID: ");
                    debugPrintln(&huart1, msg.msgid);
                    if(resLen){
                        debugPrintln(&huart1, " >>>Income_MSG.....CONFIRM");
                    }
#endif
                    HAL_UART_Transmit(&huart2, (uint8_t*)resBuf, resLen, 100);
                }
                break;
#ifdef DEBUG_MODE
//              default:
//                  debugPrintln(&huart1, " >>>Income_MSG........NO_SIMILAR");
//                  break;
#endif
            }
        }
    }
}
void Request_Datastream() {

    /*Initialize the required buffers*/
    mavlink_message_t msg;
    uint8_t buf[MAVLINK_MAX_PACKET_LEN];

    /*Pack the message*/
    mavlink_msg_request_data_stream_pack( sysid, compid, &msg, target_system, target_component, req_stream_id, req_message_rate, start_stop);
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

#ifdef DEBUG_MODE
    debugPrintln(&huart1, " MAVLink..............REQUESTED");
#endif

  HAL_UART_Transmit(&huart2, (uint8_t*)buf, len, 100);
}
/* USER CODE END 4 */
c++

1 个回答

  1. Best Answer

    Serial.available()在arduino中检查缓冲区中的数据，即 有一些（隐藏的）代码从 UART 获取数据并将其放入此缓冲区。HAL 中没有这样的代码，HAL_UART_GetState它并不能确定可用数据的可用性，而只是检查库的状态。例如，呼叫会将HAL_UART_Receive_IT状态更改为HAL_UART_STATE_BUSY_RX。

    如果您需要保留文章中的工作原理，但在 HAL 上做所有事情，那么您需要编写自己的实现avaliable()- 从 UART 获取数据并将其添加到缓冲区，然后从那里获取。

    或许把adruin作为一个软件平台离开会更容易，因为它也可以滚到STM32上，往STM32duino方向挖。

    • 2

相关问题