0

Исходный код с восхитительной статьи (https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566) о связи автопилота с дополнительным компьютером на борту.

Переделал его на свою задачу, но принцип остался прежним. Единственная разница с исходником - другой микроконтроллер. Я тестил на Arduino MINI, но его не хватит для будущих модернизаций. Так, что перевел код на рельсы STM32 где и образовалась сложность.

---------------------------------------------- ПРОБЛЕМА -----------------------------------------------

  • В коде ниже, не принимаются запрашиваемые команды!

Я стараюсь не менять подход автора статьи, но прямого аналога HAL функциям Arduino: available() не существует. Так что я попытался заменить его наиболее уместным HAL_UART_GetState().

  • Не понятно в чем проблема: условие while(...) или в конфликте нескольких HAL_UART_Receive_IT/_Transmit

Написано в STM32CubeIDE, библиотека MAVLink из статьи, микроконтроллер STM32F411CEU6 с выводом сообщений через UART (FTDI Arduino) на Termite.

#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 */

1 ответ 1

2

Serial.available() в ардуине проверяет наличие данных в буфере, т.е. есть некий (скрытый) код, который забирает данные из UART-а и кладет их в этот буфер. В HAL-е такого кода нет, а HAL_UART_GetState не определяет наличие доступных данных, а лишь проверяет статус библиотеки. Например, вызов HAL_UART_Receive_IT переведет статус в HAL_UART_STATE_BUSY_RX.

Если нужно сохранить принцип работы из статьи, но сделать все на HAL-е, то нужно написать свою реализацию avaliable() - забирать данные из UART-а и складывать их в буфер, а потом забирать оттуда.

Возможно, будет проще оставить адруину как программную платформу, ведь ее можно накатить и на STM32, покопайте в сторону STM32duino.

2
  • спасибо за ответ! До того как взял ArduinoMini, я пытался совмещать ArduinoIDE с STM32, но тогда возникли свои сложности. Буду пытаться написать свою available(), может вам попадались успешные реализации?
    – Anton-Vas
    2 дек 2021 в 9:06
  • 1
    Не попадались, да и сам если бы стал делать, то использовал бы не HAL, а прямую работу с регистрами и прерываниями, но это мне так проще. Попробуйте поискать какие-нибудь примеры приема из UART в циклический буфер, либо заглянуть в оригинал но там может быть много зависимостей, которые сложно выпилить, да и просто лишнего
    – Flexz
    2 дек 2021 в 9:21

Ваш ответ

By clicking “Отправить ответ”, you agree to our terms of service and acknowledge you have read our privacy policy.

Всё ещё ищете ответ? Посмотрите другие вопросы с метками или задайте свой вопрос.