ESP32のUDP通信のベンチマーク
- 試験環境
- 試験内容(ソースコードは記事巻末参照。前回SPP通信同様手順とした)
- 通信手順
- ASCIIコードを用い、LFで送受信における完了トリガーとする。
- 通信サイクル
- ラズパイよりデータを送信し、ESP32が受信データと同一データを送信。
- ラズパイ上で送受信データの一致確認をもってサイクル完了とした。
- 測定方法
- PC側でtime関数を使用し、上記サイクル時間を計測。
- 1サイクル内の各種オーバーヘッドは含む。但しloopのオーバーヘッドは除く。
- シリアルポートopen後の初回通信は評価対象としない。(時間が長い為)
- 通信レートは1000サイクルにおける1サイクル平均値にて算出
- 通信手順
- 試験結果
- 各送受信データ量における、N=1000の試験結果を示す。
- 表1:試験結果概要
- 各転送データ量における通信時間の分布は以下の通り。
- 図1:1~10byte時の分布
- 図2:10~100byte時の分布
- 図3:100~500byte時の分布
- 図4:500~900byte時の分布
- 各送受信データ量における、N=1000の試験結果を示す。
表1:試験結果一覧
送受信データ(byte) | 通信時間AVE(s) | 通信時間MAX(s) | 通信時間MIN(s) | 通信時間3σ(s) | 通信時間6σ(s) | 実効通信レート(bps) |
---|---|---|---|---|---|---|
1 | 0.006728 | 0.037318 | 0.002986 | 0.011635 | 0.02327 | 2378 |
2 | 0.006747 | 0.036077 | 0.002707 | 0.011704 | 0.023407 | 4743 |
3 | 0.006944 | 0.050978 | 0.002646 | 0.012661 | 0.025323 | 6912 |
4 | 0.00681 | 0.035284 | 0.002574 | 0.011763 | 0.023526 | 9398 |
5 | 0.007037 | 0.029986 | 0.002837 | 0.012069 | 0.024138 | 11368 |
10 | 0.006832 | 0.030259 | 0.002804 | 0.011145 | 0.022291 | 23419 |
20 | 0.006815 | 0.039036 | 0.00283 | 0.011928 | 0.023855 | 46955 |
30 | 0.00695 | 0.039083 | 0.002977 | 0.010938 | 0.021876 | 69065 |
40 | 0.00701 | 0.031827 | 0.002954 | 0.0109 | 0.0218 | 91298 |
50 | 0.006951 | 0.034668 | 0.002869 | 0.011069 | 0.022138 | 115091 |
100 | 0.007374 | 0.037092 | 0.003061 | 0.012287 | 0.024574 | 216979 |
150 | 0.007757 | 0.043328 | 0.003128 | 0.013567 | 0.027133 | 309398 |
200 | 0.007795 | 0.037367 | 0.00368 | 0.01239 | 0.024781 | 410520 |
250 | 0.007967 | 0.030451 | 0.00363 | 0.011485 | 0.02297 | 502071 |
300 | 0.008077 | 0.043682 | 0.004004 | 0.01274 | 0.025481 | 594280 |
350 | 0.008221 | 0.038176 | 0.00398 | 0.012072 | 0.024143 | 681182 |
400 | 0.008623 | 0.041072 | 0.004008 | 0.012782 | 0.025565 | 742201 |
450 | 0.009021 | 0.063308 | 0.00419 | 0.014533 | 0.029065 | 798138 |
500 | 0.009058 | 0.040857 | 0.004214 | 0.013759 | 0.027518 | 883197 |
600 | 0.009419 | 0.038899 | 0.004465 | 0.01333 | 0.026661 | 1019216 |
700 | 0.010279 | 0.083165 | 0.004678 | 0.017752 | 0.035503 | 1089600 |
800 | 0.011781 | 0.080902 | 0.004999 | 0.022093 | 0.044186 | 1086495 |
900 | 0.011107 | 0.061182 | 0.005582 | 0.018299 | 0.036598 | 1296480 |
- 考察
- 通信時間のバラつき
- 綺麗な1山で正規分布
- 通信自体のオーバーヘッド(1byte送信時)
- ワースト29.7.3ms(ave:6.7ms 6σ:23ms) 但し、頻度としては10ms以内が多い。
- ESP32高温時の動作異常について(温度はtemperatureRead()関数で観測)
- CPU温度が49℃を超えた際に以下現象が発生した。ESP32側はパケット送信を実施しているが、クライアント側が受信できない。この時CPUはフリーズやシャットダウンは起きていない。クライアント側からのパケット受信はできている。温度が下がれば動作は再度安定する。
- ESP32のSoftAPモードについて
- 上記高温時の不具合内容が、温度との依存関係なく頻発した。電圧ドロップ等も確認したが、3.3~3.4Vは維持出来ており、原因は不明。
- 通信時間のバラつき
- 結論
- 制御指令に対する遅延:1指令が100byte以内であれば、ワースト32ms
- 通信速度:1Mbps以上出る
- ESP32ソース
void wifi_info(){ uint8_t wifiMac[6]; esp_read_mac(wifiMac, ESP_MAC_WIFI_STA); char wifiMacChar[18] = {0}; sprintf(wifiMacChar,"%02X:%02X:%02X:%02X:%02X:%02X", wifiMac[0],wifiMac[1],wifiMac[2],wifiMac[3],wifiMac[4],wifiMac[5]); Serial.printf("MAC:%s\n",wifiMacChar); Serial.print("ip:"); Serial.println(WiFi.localIP()); Serial.print("subnet:"); Serial.println(WiFi.subnetMask()); Serial.printf("wifiTxPower:%d\n",WiFi.getTxPower()); } void wifiNonAP_setup(){ const char ssid[]= "ssid"; const char pass[]= "sspass"; WiFi.begin(ssid,pass); while (WiFi.status() != WL_CONNECTED);{ delay(500); Serial.println("wifi_connect_ok"); } void udp_setup(){ const int localport = 10000; WiFi.setTxPower(WIFI_POWER_19_5dBm); wifiNonAP_setup(); esp_wifi_set_ps(WIFI_PS_NONE); wifi_info(); udp.begin(localport); } void setup() { Serial.begin(115200); Serial.printf("cpuFreq(Mhz):%d\n",ets_get_cpu_frequency()); Serial.printf("cputemp:%f\n",temperatureRead()); udp_setup(); Serial.println("bootUpFin"); } void loop() { const char *remoteIp = "xxx.xxx.xxx.xxx"; const int remoteUdpPort = 10001; String btRxBuff; String udpRxBuff; while(true){ if (udp.parsePacket()){ udpRxBuff = udp.readStringUntil('\n'); udp.beginPacket(remoteIp, remoteUdpPort); udp.print(udpRxBuff + '\n'); udp.endPacket(); } } }
- ラズパイ
import time import socket import csv serv_ip = ("xxx.xxx.xxx.xxx", 10000) local_ip = ("xxx.xxx.xxx.xxx", 10001) tx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) rx_sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) rx_sock.bind(local_ip) def udp_tx(tx_data="", chk=1): tx_data = tx_data + '\n' tx_data_b = tx_data.encode('utf-8') tx_sock.sendto(tx_data_b, serv_ip) if chk == 1: rx_data_b, add = rx_sock.recvfrom(1024) rx_data = rx_data_b.decode('utf-8') if tx_data_b == rx_data_b: return True else: print('err') return None else: return True def udp_benchmark(loop=10, byte=1): result_ls = [byte] data = 0 send_data = "" for n in range(byte): send_data += str(data) data += 1 if data >= 9: data = 0 tx_result = udp_tx(tx_data=send_data) for n in range(loop): start = time.time() tx_result = udp_tx(tx_data=send_data) ct = time.time() - start if tx_result is None: result_ls.append(-1) else: result_ls.append(round(ct, 6)) return result_ls for n in [1, 2, 3, 4, 5, 10, 20, 30, 40, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 600, 700, 800, 900, 1000]: result_ls = udp_benchmark(loop=1000, byte=n) print(result_ls) with open('udp_benchmark_pc.csv', 'a', newline='') as f: writer = csv.writer(f) writer.writerow(result_ls) f.close() tx_sock.close() rx_sock.close()
ESP32とラズパイでBluetoothによるSPP通信のベンチマーク
- 目的
- ESP32とラズパイでBluetoothのSPP通信を行った場合遅延と実効通信レートがどの程度になるか調査をする。
- 試験環境
- 試験内容(ソースコードは記事巻末参照)
- 通信手順
- ASCIIコードを用い、LFで送受信における完了トリガーとする。
- 通信サイクル
- ラズパイよりデータを送信し、ESP32が受信データと同一データを送信。
- ラズパイ上で送受信データの一致確認をもってサイクル完了とした。
- 測定方法
- ラズパイ側でtime関数を使用し、上記サイクル時間を計測。
- 1サイクル内の各種オーバーヘッドは含む。但しloopのオーバーヘッドは除く。
- シリアルポートopen後の初回通信は評価対象としない。(時間が長い為)
- 通信レートは1000サイクルにおける1サイクル平均値にて算出
- 通信手順
- 試験結果
- 各送受信データ量における、N=1000の試験結果を示す。
- 表1:試験結果概要
- 各転送データ量における通信時間の分布は以下の通り。
- 図1:1~10byte時の分布
- 図2:10~100byte時の分布
- 図3:100~500byte時の分布
- 図4:500~900byte時の分布
- 各送受信データ量における、N=1000の試験結果を示す。
表1:試験結果一覧
送受信データ(byte) | 通信時間AVE(s) | 通信時間MAX(s) | 通信時間MIN(s) | 通信時間3σ(s) | 通信時間6σ(s) | 実効通信レート(bps) |
---|---|---|---|---|---|---|
1 | 0.013288 | 0.071321 | 0.006205 | 0.032500 | 0.065000 | 1204 |
2 | 0.013523 | 0.081167 | 0.006226 | 0.033617 | 0.067233 | 2366 |
3 | 0.013145 | 0.058818 | 0.007148 | 0.032047 | 0.064094 | 3652 |
4 | 0.01379 | 0.063877 | 0.006835 | 0.033823 | 0.067645 | 4641 |
5 | 0.013136 | 0.057554 | 0.006931 | 0.031898 | 0.063795 | 6090 |
10 | 0.013178 | 0.057569 | 0.007301 | 0.031447 | 0.062894 | 12141 |
20 | 0.015249 | 0.065444 | 0.008145 | 0.033261 | 0.066522 | 20985 |
30 | 0.017178 | 0.066215 | 0.008599 | 0.035967 | 0.071934 | 27943 |
40 | 0.017413 | 0.090023 | 0.009469 | 0.035429 | 0.070858 | 36754 |
50 | 0.019243 | 0.08304 | 0.009867 | 0.035136 | 0.070273 | 41574 |
100 | 0.033166 | 0.085077 | 0.014676 | 0.036745 | 0.073490 | 48242 |
150 | 0.036418 | 0.096614 | 0.015962 | 0.032540 | 0.065080 | 65901 |
200 | 0.036001 | 0.082626 | 0.018734 | 0.031361 | 0.062723 | 88886 |
250 | 0.037932 | 0.087403 | 0.022059 | 0.027688 | 0.055376 | 105452 |
300 | 0.039324 | 0.093719 | 0.024318 | 0.025414 | 0.050828 | 122063 |
350 | 0.041409 | 0.076375 | 0.026813 | 0.024264 | 0.048528 | 135236 |
400 | 0.044113 | 0.086197 | 0.028701 | 0.028282 | 0.056563 | 145082 |
450 | 0.057283 | 0.098023 | 0.032105 | 0.038603 | 0.077206 | 125692 |
500 | 0.063707 | 0.110676 | 0.034387 | 0.034159 | 0.068317 | 125575 |
600 | 0.067726 | 0.137092 | 0.040482 | 0.032502 | 0.065004 | 141748 |
700 | 0.075531 | 0.173518 | 0.044626 | 0.033677 | 0.067354 | 148283 |
800 | 0.079246 | 0.128892 | 0.049892 | 0.030826 | 0.061653 | 161522 |
900 | 0.095119 | 0.166947 | 0.056227 | 0.037209 | 0.074418 | 151389 |
- 考察
- 通信時間のバラつき
- 正規分布ではなく、2山になっている。6σでの想定が妥当である。
- 通信自体のオーバーヘッド(1byte送信時)
- ワースト78.3ms(ave:13.3ms 6σ:65ms) 但し、頻度としては10msが多い。
- 通信時間のバラつき
- 結論(一回辺り100byte以内の送受信による制御を想定)
- 制御指令に対する遅延:ワースト106ms
- ESP32ソース
#include <Arduino.h> #include <BluetoothSerial.h> BluetoothSerial SerialBT; String btrx_buff; void spp_setup(){ uint8_t macBT[6]; char btname[11]; esp_read_mac(macBT, ESP_MAC_BT); sprintf(btname, "ESP32-%02X%02X", macBT[4], macBT[5]); if (SerialBT.begin(btname)) { Serial.println("start_spp"); } } void setup() { Serial.begin(115200); spp_setup(); } void loop() { btrx_buff = SerialBT.readStringUntil('\n'); SerialBT.print(btrx_buff + '\n'); //Serial.println(btrx_buff); }
- ラズパイ
import serial import time import csv bt_spp = serial.Serial(port='/dev/rfcomm0', baudrate=115200, timeout=None) def spp_tx(tx_buff=None,chk=1): tx_buff = tx_buff + '\n' tx_buff_b = tx_buff.encode('utf-8') bt_spp.write(tx_buff_b) if chk == 1: rx_data_b = bt_spp.readline() rx_data = rx_data_b.decode('utf-8') if tx_buff == rx_data: return True else: return None elif chk == 2: rx_data_b = bt_spp.readline() rx_data = rx_data_b.decode('utf-8') if rx_data == '\n': return True else: return None else: return True def spp_benchimark(loop=10,byte=1): ct = 0 result_ls =[byte] data = 0 send_data ="" for n in range(byte): send_data += str(data) data +=1 if data >=9: data = 0 tx_result = spp_tx(tx_buff=send_data) for n in range(loop): start = time.time() tx_result = spp_tx(tx_buff=send_data) ct = time.time()-start if tx_result is None: result_ls.append(-1) else: result_ls.append(round(ct,6)) return result_ls for n in [1,2,3,4,5,10,20,30,40,50,100,150,200,250,300,350,400,450,500,600,700,800,900]: result_ls = spp_benchimark(loop=1000,byte=n) with open('data.csv','a') as f: writer = csv.writer(f) writer.writerow(result_ls) bt_spp.close()
Nucleo64_F446RE用シールド基板完成
Nucleo64をそのまま使い、ラジコン用のシールド基板を作成。
主電源:12~13V前後のバッテリーを使用。
5V電源:村田製降圧型DCDCコンバータ OKL-T/6-W12N-C
モータードライブIC:DRV8871
6軸ジャイロ:MPU-6050(GY-521)
■通信関係
メインCPU通信用SPI(SPI2)・・・J3端子台・ラズパイと繋ぎこみ
メインCPU通信用UART(UART6)・・・J3端子台・SPIとジャンパで切替
ジャイロセンサ通信用I2C(I2C1)・・・基板上MPU-6050と接続
予備SPI(SPI3)・・・J10ピンソケット出力
予備I2C(I2C2/I2C3)・・・J8/J9ピンソケット出力
■モーター制御
AB相エンコーダ・・・J2ピンソケット 5Vトレラント接続
サーボモータ・・・J4,J5ピンソケット
■センサー
バッテリー電圧監視・・・抵抗分圧でADC1入力(R1:47kΩ R3:10kΩ)
5V電圧監視・・・抵抗分圧でADC2入力(R2,R3:10kΩ)
超音波距離センサーSR04・・・J6,J7ピンソケット入力(5Vトレラント)
モーター制御基板設計変更 STM32F303K8にて
モーター制御基板をラズパイPicoからSTM32F303K8へ変更
理由
2.100LPR程度のエンコーダーを使えるようにしたい
3.そもそも処理能力がカツカツになった
STM32F303K8へ変更後
■通信
>SPI通信10MHzのスレーブデバイス
>DMA転送で通信処理負荷低減
>設定値等の通信は、レジスタマップとして複数同時処理できるようにした
>ペリフェラルのエンコーダを使用
■RTOS
>オーバーヘッドのデメリットの方が多く、
STM32F303K8ではメモリがギリギリになりそうなので不採用とした。
■構成
以下の通り、ギリギリ足ります。現状空きPINは2点。
・I2C×1・・・ジャイロ用
・SPI全二重×1・・・主制御装置通信用(ラズパイ)
・動力モーター用PWM(TIM1、CH1,2)
・サーボモータ用PWM(TIM2、CH1,2)
・エンコーダー用タイマ(TIM3)・・・入力は5V耐圧端子にて
・GPIO×3・・・超音波センサーSR04用、TRIGは共用
・制御系5V及びバッテリー電圧監視用ADC×2(ADC1,IN4,IN11)
注意点:Nucleoを使用する場合、I2CとSPIを同時使用する場合
裏面ショートジャンパ SB16とSB18は取り除く必要があります。
これに気づくのに一週間位溶かしました。
完成はしてませんが、概ね形になってきたのでソースご参考迄に
最初はLLで書こうかと思いましたが、後でF4に変更したりの可能性もあったので
HALで書いてあります。
SPI通信は1フレーム8bit固定×任意フレーム数として通信できるように
マスターから送信時に以下のようにしました。
「ヘッダ、開始アドレス0xXX、R/Wアドレス数」
ヘッダは、読み込み0x10、書き込み0x20。
将来的に、このヘッダを拡張してバースト処理できるようにしようと思います。
STM32F303K8 作りかけソース20211115
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* <h2><center>© Copyright (c) 2021 STMicroelectronics.
* All rights reserved.</center></h2>
*
* This software component is licensed by ST under BSD 3-Clause license,
* the "License"; You may not use this file except in compliance with the
* License. You may obtain a copy of the License at:
* opensource.org/licenses/BSD-3-Clause
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "string.h"
#include "math.h"
#include "inttypes.h"
#define ADDR_ERR_1 0x00
#define ADDR_ERR_2 0x01
#define ADDR_ERR_3 0x02
#define ADDR_ERR_4 0x03
#define ADDR_ERR_5 0x04
#define ADDR_ERR_6 0x05
#define ADDR_ERR_7 0x06
#define ADDR_ERR_8 0x07
#define ADDR_ERR_9 0x08
#define ADDR_ERR_10 0x09
#define ADDR_RPS_SET_H 0x0A
#define ADDR_RPS_SET_L 0x0B
#define ADDR_TRQ_OVER 0x0C
#define ADDR_ROT_RST 0x0D
#define ADDR_STP_ROT_H 0x0E
#define ADDR_STP_ROT_L 0x0F
#define ADDR_ROT_SUM_H 0x10
#define ADDR_ROT_SUM_L 0x11
#define ADDR_RPS_RAW_H 0x12
#define ADDR_RPS_RAW_L 0x13
#define ADDR_RPS_AVE_H 0x14
#define ADDR_RPS_AVE_L 0x15
#define ADDR_RPS_MAX_H 0x16
#define ADDR_RPS_MAX_L 0x17
#define ADDR_RPS_MIN_H 0x18
#define ADDR_RPS_MIN_L 0x19
#define ADDR_DTY_RAW_H 0x1A
#define ADDR_DTY_RAW_L 0x1B
#define ADDR_DTY_AVE_H 0x1C
#define ADDR_DTY_AVE_L 0x1D
#define ADDR_DTY_MAX_H 0x1E
#define ADDR_DTY_MAX_L 0x1F
#define ADDR_DTY_MIN_H 0x20
#define ADDR_DTY_MIN_L 0x21
#define ADDR_DRV_MEM 0x22
#define ADDR_AVE_TIM 0x23
#define ADDR_PID_KP_H 0x24
#define ADDR_PID_KP_L 0x25
#define ADDR_PID_KI_H 0x26
#define ADDR_PID_KI_L 0x27
#define ADDR_PID_KD_H 0x28
#define ADDR_PID_KD_L 0x29
#define ADDR_DT1_REQ 0x30
#define ADDR_DT1_DST_H 0x31
#define ADDR_DT1_DST_L 0x32
#define ADDR_DT2_REQ 0x33
#define ADDR_DT2_DST_H 0x34
#define ADDR_DT2_DST_L 0x35
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
I2C_HandleTypeDef hi2c1;
RTC_HandleTypeDef hrtc;
SPI_HandleTypeDef hspi1;
DMA_HandleTypeDef hdma_spi1_rx;
DMA_HandleTypeDef hdma_spi1_tx;
TIM_HandleTypeDef htim1;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim6;
TIM_HandleTypeDef htim7;
TIM_HandleTypeDef htim17;
UART_HandleTypeDef huart2;
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_DMA_Init(void);
static void MX_SPI1_Init(void);
static void MX_I2C1_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM6_Init(void);
static void MX_TIM7_Init(void);
static void MX_TIM2_Init(void);
static void MX_RTC_Init(void);
static void MX_ADC1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM17_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
int _write(int file, char *ptr, int len){
HAL_UART_Transmit(&huart2,(uint8_t *)ptr,len,10);
return len;
}
uint8_t param_arry[200] = {0};
enum {
SPI_STATE_READY,SPI_STATE_WAIT_COMMAND,SPI_STATE_GET_COMMAND,SPI_STATE_WAIT_READ,SPI_STATE_WAIT_WRITE,SPI_STATE_COMMAND_ERR
};
enum{
DIST1_READY,DIST1_INIT_START,DIST1_INIT_END,DIST1_MES_START,DIST1_TRG_WAIT,DIST1_ECH_WAIT,DIST1_MES_END
};
enum{
DIST2_READY,DIST2_INIT_START,DIST2_INIT_END,DIST2_MES_START,DIST2_TRG_WAIT,DIST2_ECH_WAIT,DIST2_MES_END
};
uint16_t dist1_st = 0, dist1_ed = 0;
uint16_t dist2_st = 0, dist2_ed = 0;
uint8_t dist1_state = DIST1_READY;
uint8_t dist2_state = DIST2_READY;
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
setbuf(stdout, NULL);
param_arry[ADDR_AVE_TIM] = 50;
param_arry[ADDR_PID_KP_H] = ((uint16_t)(600)) >> 8;
param_arry[ADDR_PID_KP_L] = ((uint16_t)(600)) & 0x00FF;
param_arry[ADDR_PID_KI_H] = ((uint16_t)(2)) >> 8;
param_arry[ADDR_PID_KI_L] = ((uint16_t)(2)) & 0x00FF;
param_arry[ADDR_PID_KD_H] = ((uint16_t)(300)) >> 8;
param_arry[ADDR_PID_KD_L] = ((uint16_t)(300)) & 0x00FF;
/* 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_USART2_UART_Init();
MX_DMA_Init();
MX_SPI1_Init();
MX_I2C1_Init();
MX_TIM1_Init();
MX_TIM6_Init();
MX_TIM7_Init();
MX_TIM2_Init();
MX_RTC_Init();
MX_ADC1_Init();
MX_TIM3_Init();
MX_TIM17_Init();
/* USER CODE BEGIN 2 */
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, 0);
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1, 0);
__HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2, 0);
HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_1);
HAL_TIM_Encoder_Start( &htim3, TIM_CHANNEL_2);
HAL_TIM_Base_Start_IT(&htim6);
HAL_TIM_Base_Start_IT(&htim7);
HAL_TIM_Base_Start_IT(&htim17);
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
uint8_t spiState = SPI_STATE_READY;
while (1)
{
/*spi_start*/
static uint8_t command_rxbuf[2] = {};
static uint8_t spi_txbuf[128] = {0};
static uint8_t spi_rxbuf[128] = {0};
if (spiState == SPI_STATE_READY) {
HAL_SPI_Receive_DMA(&hspi1, command_rxbuf,3);
spiState = SPI_STATE_WAIT_COMMAND;
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_COMMAND) {
spiState = SPI_STATE_GET_COMMAND;
}
if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x10 && command_rxbuf[2] >=1) {
spiState = SPI_STATE_WAIT_READ;
for (int var = 0; var < command_rxbuf[2]; ++var) {
spi_txbuf[var] = param_arry[command_rxbuf[1] + var];
}
HAL_SPI_Transmit_DMA(&hspi1, spi_txbuf, command_rxbuf[2]);
}
else if (spiState == SPI_STATE_GET_COMMAND && command_rxbuf[0] == 0x20 && command_rxbuf[2] >=1) {
spiState = SPI_STATE_WAIT_WRITE;
HAL_SPI_Receive_DMA(&hspi1, spi_rxbuf, command_rxbuf[2]);
}
else if (spiState == SPI_STATE_GET_COMMAND) {
spiState = SPI_STATE_COMMAND_ERR;
printf("command_err\r\n");
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_READ) {
spiState = SPI_STATE_READY;
}
if (HAL_SPI_GetState(&hspi1) == HAL_SPI_STATE_READY && spiState == SPI_STATE_WAIT_WRITE) {
for (int var = 0; var < command_rxbuf[2]; ++var) {
param_arry[command_rxbuf[1] + var] = spi_rxbuf[var];
}
spiState = SPI_STATE_READY;
}
/*spi_end*/
/*dist_mes*/
uint8_t dist_trg_pin_data = HAL_GPIO_ReadPin(DIST_TRG_GPIO_Port,DIST_TRG_Pin);
uint8_t dist1_ech_pin_data = HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port,DIST1_ECH_Pin);
uint16_t dist1_dist = (int16_t)(((param_arry[ADDR_DT1_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT1_DST_H] & 0x00FF));
uint16_t dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
if (param_arry[ADDR_DT1_REQ] != 0 && dist1_state == DIST1_READY && dist_trg_pin_data == GPIO_PIN_RESET && dist1_ech_pin_data == GPIO_PIN_RESET
&& dist2_state == DIST2_READY) {
dist1_state = DIST1_INIT_START;
param_arry[ADDR_DT1_DST_H] = 0;
param_arry[ADDR_DT1_DST_L] = 0;
}else if (dist1_state == DIST1_INIT_START) {
dist1_state = DIST1_INIT_END;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
__HAL_TIM_SET_COUNTER(&htim17,0);
dist1_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
}else if (dist1_state == DIST1_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
dist1_state = DIST1_TRG_WAIT;
}else if (dist1_state == DIST1_TRG_WAIT && dist1_cycle_time > 2) {
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
dist1_state = DIST1_ECH_WAIT;
}else if (dist1_state == DIST1_MES_END) {
dist1_state = DIST1_READY;
param_arry[ADDR_DT1_REQ] = 0;
}else if (dist1_state >= DIST1_INIT_END && dist1_cycle_time > 3000) {
dist1_dist = -1;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
param_arry[ADDR_DT1_REQ] = 0;
dist1_state = DIST1_READY;
}
/*dist2_mes*/
uint8_t dist2_ech_pin_data = HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port,DIST2_ECH_Pin);
uint16_t dist2_dist = (int16_t)(((param_arry[ADDR_DT2_DST_H] << 8) & 0xFF00) | (param_arry[ADDR_DT2_DST_H] & 0x00FF));
uint16_t dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
if (param_arry[ADDR_DT2_REQ] != 0 && dist2_state == DIST2_READY && dist1_state == DIST1_READY
&& dist_trg_pin_data == GPIO_PIN_RESET && dist2_ech_pin_data == GPIO_PIN_RESET){
dist2_state = DIST2_INIT_START;
param_arry[ADDR_DT2_DST_H] = 0;
param_arry[ADDR_DT2_DST_L] = 0;
}else if (dist2_state == DIST2_INIT_START) {
dist2_state = DIST2_INIT_END;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_SET);
__HAL_TIM_SET_COUNTER(&htim17,0);
dist2_cycle_time = __HAL_TIM_GET_COUNTER(&htim17);
}else if (dist2_state == DIST2_INIT_END && dist_trg_pin_data == GPIO_PIN_SET) {
dist2_state = DIST2_TRG_WAIT;
}else if (dist2_state == DIST2_TRG_WAIT && dist2_cycle_time > 2) {
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
dist2_state = DIST2_ECH_WAIT;
}else if (dist2_state == DIST2_MES_END) {
dist2_state = DIST2_READY;
param_arry[ADDR_DT2_REQ] = 0;
}else if (dist2_state >= DIST2_INIT_END && dist2_cycle_time > 3000) {
dist2_dist = -1;
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
param_arry[ADDR_DT2_REQ] = 0;
dist2_state = DIST2_READY;
}
/* 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};
RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_LSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.LSIState = RCC_LSI_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
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_2) != HAL_OK)
{
Error_Handler();
}
PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_I2C1|RCC_PERIPHCLK_RTC
|RCC_PERIPHCLK_TIM1|RCC_PERIPHCLK_ADC12;
PeriphClkInit.Adc12ClockSelection = RCC_ADC12PLLCLK_DIV1;
PeriphClkInit.I2c1ClockSelection = RCC_I2C1CLKSOURCE_HSI;
PeriphClkInit.RTCClockSelection = RCC_RTCCLKSOURCE_LSI;
PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLK_PLLCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC1_Init(void)
{
/* USER CODE BEGIN ADC1_Init 0 */
/* USER CODE END ADC1_Init 0 */
ADC_MultiModeTypeDef multimode = {0};
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC1_Init 1 */
/* USER CODE END ADC1_Init 1 */
/** Common config
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_ASYNC_DIV1;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = ADC_SCAN_DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DMAContinuousRequests = DISABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
hadc1.Init.LowPowerAutoWait = DISABLE;
hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
Error_Handler();
}
/** Configure the ADC multi-mode
*/
multimode.Mode = ADC_MODE_INDEPENDENT;
if (HAL_ADCEx_MultiModeConfigChannel(&hadc1, &multimode) != HAL_OK)
{
Error_Handler();
}
/** Configure Regular Channel
*/
sConfig.Channel = ADC_CHANNEL_4;
sConfig.Rank = ADC_REGULAR_RANK_1;
sConfig.SingleDiff = ADC_SINGLE_ENDED;
sConfig.SamplingTime = ADC_SAMPLETIME_1CYCLE_5;
sConfig.OffsetNumber = ADC_OFFSET_NONE;
sConfig.Offset = 0;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC1_Init 2 */
/* USER CODE END ADC1_Init 2 */
}
/**
* @brief I2C1 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C1_Init(void)
{
/* USER CODE BEGIN I2C1_Init 0 */
/* USER CODE END I2C1_Init 0 */
/* USER CODE BEGIN I2C1_Init 1 */
/* USER CODE END I2C1_Init 1 */
hi2c1.Instance = I2C1;
hi2c1.Init.Timing = 0x2000090E;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c1) != HAL_OK)
{
Error_Handler();
}
/** Configure Analogue filter
*/
if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
{
Error_Handler();
}
/** Configure Digital filter
*/
if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C1_Init 2 */
/* USER CODE END I2C1_Init 2 */
}
/**
* @brief RTC Initialization Function
* @param None
* @retval None
*/
static void MX_RTC_Init(void)
{
/* USER CODE BEGIN RTC_Init 0 */
/* USER CODE END RTC_Init 0 */
/* USER CODE BEGIN RTC_Init 1 */
/* USER CODE END RTC_Init 1 */
/** Initialize RTC Only
*/
hrtc.Instance = RTC;
hrtc.Init.HourFormat = RTC_HOURFORMAT_24;
hrtc.Init.AsynchPrediv = 127;
hrtc.Init.SynchPrediv = 255;
hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;
hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;
hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;
if (HAL_RTC_Init(&hrtc) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN RTC_Init 2 */
/* USER CODE END RTC_Init 2 */
}
/**
* @brief SPI1 Initialization Function
* @param None
* @retval None
*/
static void MX_SPI1_Init(void)
{
/* USER CODE BEGIN SPI1_Init 0 */
/* USER CODE END SPI1_Init 0 */
/* USER CODE BEGIN SPI1_Init 1 */
/* USER CODE END SPI1_Init 1 */
/* SPI1 parameter configuration*/
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_SLAVE;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH;
hspi1.Init.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_HARD_INPUT;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi1.Init.CRCPolynomial = 7;
hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;
hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI1_Init 2 */
/* USER CODE END SPI1_Init 2 */
}
/**
* @brief TIM1 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM1_Init(void)
{
/* USER CODE BEGIN TIM1_Init 0 */
/* USER CODE END TIM1_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};
/* USER CODE BEGIN TIM1_Init 1 */
/* USER CODE END TIM1_Init 1 */
htim1.Instance = TIM1;
htim1.Init.Prescaler = 0;
htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
htim1.Init.Period = 2559;
htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim1.Init.RepetitionCounter = 0;
htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim1, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim1) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
sBreakDeadTimeConfig.DeadTime = 0;
sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
sBreakDeadTimeConfig.BreakFilter = 0;
sBreakDeadTimeConfig.Break2State = TIM_BREAK2_DISABLE;
sBreakDeadTimeConfig.Break2Polarity = TIM_BREAK2POLARITY_HIGH;
sBreakDeadTimeConfig.Break2Filter = 0;
sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
if (HAL_TIMEx_ConfigBreakDeadTime(&htim1, &sBreakDeadTimeConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM1_Init 2 */
/* USER CODE END TIM1_Init 2 */
HAL_TIM_MspPostInit(&htim1);
}
/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 0;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 4294967295;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* @brief TIM3 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM3_Init(void)
{
/* USER CODE BEGIN TIM3_Init 0 */
/* USER CODE END TIM3_Init 0 */
TIM_Encoder_InitTypeDef sConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM3_Init 1 */
/* USER CODE END TIM3_Init 1 */
htim3.Instance = TIM3;
htim3.Init.Prescaler = 0;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 65535;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sConfig.IC1Filter = 0;
sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sConfig.IC2Filter = 0;
if (HAL_TIM_Encoder_Init(&htim3, &sConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM3_Init 2 */
/* USER CODE END TIM3_Init 2 */
}
/**
* @brief TIM6 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM6_Init(void)
{
/* USER CODE BEGIN TIM6_Init 0 */
/* USER CODE END TIM6_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM6_Init 1 */
/* USER CODE END TIM6_Init 1 */
htim6.Instance = TIM6;
htim6.Init.Prescaler = 64;
htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
htim6.Init.Period = 9999;
htim6.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM6_Init 2 */
/* USER CODE END TIM6_Init 2 */
}
/**
* @brief TIM7 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM7_Init(void)
{
/* USER CODE BEGIN TIM7_Init 0 */
/* USER CODE END TIM7_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM7_Init 1 */
/* USER CODE END TIM7_Init 1 */
htim7.Instance = TIM7;
htim7.Init.Prescaler = 64000;
htim7.Init.CounterMode = TIM_COUNTERMODE_UP;
htim7.Init.Period = 100;
htim7.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim7) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim7, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM7_Init 2 */
/* USER CODE END TIM7_Init 2 */
}
/**
* @brief TIM17 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM17_Init(void)
{
/* USER CODE BEGIN TIM17_Init 0 */
/* USER CODE END TIM17_Init 0 */
/* USER CODE BEGIN TIM17_Init 1 */
/* USER CODE END TIM17_Init 1 */
htim17.Instance = TIM17;
htim17.Init.Prescaler = 639;
htim17.Init.CounterMode = TIM_COUNTERMODE_UP;
htim17.Init.Period = 65535;
htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim17.Init.RepetitionCounter = 0;
htim17.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim17) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM17_Init 2 */
/* USER CODE END TIM17_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 = 38400;
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;
huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
if (HAL_UART_Init(&huart2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART2_Init 2 */
/* USER CODE END USART2_Init 2 */
}
/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Channel2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel2_IRQn);
/* DMA1_Channel3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Channel3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA1_Channel3_IRQn);
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOF_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(DIST_TRG_GPIO_Port, DIST_TRG_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(LED_GPIO_Port, LED_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin : TEST_Pin */
GPIO_InitStruct.Pin = TEST_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(TEST_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : DIST2_ECH_Pin DIST1_ECH_Pin */
GPIO_InitStruct.Pin = DIST2_ECH_Pin|DIST1_ECH_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pin : DIST_TRG_Pin */
GPIO_InitStruct.Pin = DIST_TRG_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(DIST_TRG_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : LED_Pin */
GPIO_InitStruct.Pin = LED_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
}
/* USER CODE BEGIN 4 */
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_TXRX_COMP;
//printf("spi_txrx_comp\r\n");
}
void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_RX_COMP;
//printf("spi_rx_comp\r\n");
}
void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_TX_COMP;
//printf("spi_tx_comp\r\n");
}
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi1){
//spiState = SPI_ERR;
printf("spi_err\r\n");
}
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin){
if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_SET && dist1_state == DIST1_ECH_WAIT) {
dist1_state = DIST1_MES_START;
dist1_st = __HAL_TIM_GET_COUNTER(&htim17);
//printf("%d",dist1_st);
}
if (GPIO_Pin == DIST1_ECH_Pin && HAL_GPIO_ReadPin(DIST1_ECH_GPIO_Port, DIST1_ECH_Pin) == GPIO_PIN_RESET && dist1_state == DIST1_MES_START) {
dist1_ed = __HAL_TIM_GET_COUNTER(&htim17);
int16_t dist1_dist = ((float)(dist1_ed - dist1_st) * 3.4f) / 2.0f;
param_arry[ADDR_DT1_DST_H] = (dist1_dist) >> 8;
param_arry[ADDR_DT1_DST_L] = (dist1_dist) & 0x00FF;
dist1_state = DIST1_MES_END;
}
if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_SET && dist2_state == DIST2_ECH_WAIT) {
dist2_state = DIST2_MES_START;
dist2_st = __HAL_TIM_GET_COUNTER(&htim17);
//printf("%d",dist1_st);
}
if (GPIO_Pin == DIST2_ECH_Pin && HAL_GPIO_ReadPin(DIST2_ECH_GPIO_Port, DIST2_ECH_Pin) == GPIO_PIN_RESET && dist2_state == DIST2_MES_START) {
dist2_ed = __HAL_TIM_GET_COUNTER(&htim17);
int16_t dist2_dist = ((float)(dist2_ed - dist2_st) * 3.4f) / 2.0f;
param_arry[ADDR_DT2_DST_H] = (dist2_dist) >> 8;
param_arry[ADDR_DT2_DST_L] = (dist2_dist) & 0x00FF;
dist2_state = DIST2_MES_END;
}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
//HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
if (htim == &htim6) {
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_SET);
const float dt = 10.0f;
const float rot_pls = 52;
const float pwm_period = 2559.0f;
int16_t cnt_raw = 0;
static float rps_arr[50] = {}, duty_arr[50] = {};
static float rps_max=0, rps_min=0, duty_max=0, duty_min=0;
static float rps_raw = 0,rps_ave = 0;
static float duty_raw = 0,duty = 0,duty_ave = 0;
float rps_sum = 0, duty_sum=0;
static int64_t cnt_sum = 0;
static int16_t rot_sum = 0;
static uint8_t ave_count = 0;
static uint16_t trq_over_cont = 0;
static float P,I,D,preP=0;
float ave_point = param_arry[ADDR_AVE_TIM];
float set_rps = (float)(int16_t)(((param_arry[ADDR_RPS_SET_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_SET_L] & 0x00FF));
int16_t stop_rot = (int16_t)(((param_arry[ADDR_STP_ROT_H] << 8) & 0xFF00) | (param_arry[ADDR_STP_ROT_L] & 0x00FF));
float kp = (float)(int16_t)(((param_arry[ADDR_PID_KP_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KP_L] & 0x00FF)) * 0.01f;
float ki = (float)(int16_t)(((param_arry[ADDR_PID_KI_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KI_L] & 0x00FF)) * 0.01f;
float kd = (float)(int16_t)(((param_arry[ADDR_PID_KD_H] << 8) & 0xFF00) | (param_arry[ADDR_PID_KD_L] & 0x00FF)) * 0.01f;
cnt_raw = __HAL_TIM_GET_COUNTER(&htim3);
__HAL_TIM_SET_COUNTER(&htim3,0);
if (ave_count > ave_point) {ave_count = 0;}
if (param_arry[ADDR_ROT_RST] != 0x00) {
cnt_sum = 0;
param_arry[ADDR_ROT_RST] = 0x00;
}
cnt_sum += cnt_raw;
rot_sum = cnt_sum / rot_pls;
rps_raw = ((float)cnt_raw / rot_pls) * (1000.0f/dt);
rps_arr[ave_count] = rps_raw;
//PID
P = set_rps - rps_raw;
I += (P + preP) / 2.0f * dt;
D = (P - preP) / dt;
preP = P;
if (fabs(rps_ave) < 0.001 && set_rps == 0) {
P = 0;
I = 0;
D = 0;
}
duty_raw = kp * P + ki *I + kd * D;
if (duty_raw >= 100) {
duty_raw = 100;
}else if (duty <= -100) {
duty_raw = -100;
}
duty = duty_raw;
duty_arr[ave_count] = duty;
for (int var = 0; var < ave_point; ++var) {duty_sum += duty_arr[var];}
duty_ave = duty_sum / ave_point;
for (int var = 0; var < ave_point; ++var) {rps_sum += rps_arr[var];}
rps_ave = rps_sum / ave_point;
if (rps_max < rps_ave) {rps_max = rps_ave;}
if (rps_min > rps_ave) {rps_min = rps_ave;}
if (duty_max < duty) {duty_max = duty;}
if (duty < duty_min) {duty_min = duty;}
if (param_arry[ADDR_DRV_MEM] != 0) {
duty_max = duty;
duty_min = duty;
rps_max = rps_ave;
rps_min = rps_ave;
param_arry[ADDR_DRV_MEM] = 0x00;
}
param_arry[ADDR_ROT_SUM_H] = (rot_sum >> 8);
param_arry[ADDR_ROT_SUM_L] = (rot_sum >> 0);
param_arry[ADDR_RPS_RAW_H] = ((int16_t)(rps_raw * 100.0f)) >> 8;
param_arry[ADDR_RPS_RAW_L] = ((int16_t)(rps_raw * 100.0f)) >> 0;
param_arry[ADDR_RPS_AVE_H] = ((int16_t)(rps_ave * 100.0f)) >> 8;
param_arry[ADDR_RPS_AVE_L] = ((int16_t)(rps_ave * 100.0f)) >> 0;
param_arry[ADDR_RPS_MAX_H] = ((int16_t)(rps_max * 100.0f)) >> 8;
param_arry[ADDR_RPS_MAX_L] = ((int16_t)(rps_max * 100.0f)) >> 0;
param_arry[ADDR_RPS_MIN_H] = ((int16_t)(rps_min * 100.0f)) >> 8;
param_arry[ADDR_RPS_MIN_L] = ((int16_t)(rps_min * 100.0f)) >> 0;
param_arry[ADDR_DTY_AVE_H] = ((int16_t)(duty_ave * 100.0f)) >> 8;
param_arry[ADDR_DTY_AVE_L] = ((int16_t)(duty_ave * 100.0f)) >> 0;
param_arry[ADDR_DTY_MAX_H] = ((int16_t)(duty_max * 100.0f)) >> 8;
param_arry[ADDR_DTY_MAX_L] = ((int16_t)(duty_max * 100.0f)) >> 0;
param_arry[ADDR_DTY_MIN_H] = ((int16_t)(duty_min * 100.0f)) >> 8;
param_arry[ADDR_DTY_MIN_L] = ((int16_t)(duty_min * 100.0f)) >> 0;
ave_count += 1;
/*PWM*/
int pwm_count = (1-(fabs(duty)/100)) * pwm_period;
if (param_arry[ADDR_TRQ_OVER] != 0) {
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
if ((stop_rot > 0 && rot_sum >= stop_rot) || (stop_rot < 0 && rot_sum <= stop_rot)) {
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
if (duty >= 0) {
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, (int)pwm_period);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, pwm_count);
}else {
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1, pwm_count);
__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2, (int)pwm_period);
}
if (fabs(duty) >= 99 && fabs(rps_raw) < 0.5f) {
trq_over_cont += 1;
if (trq_over_cont > 1000/dt) {
param_arry[ADDR_TRQ_OVER] = 0x01;
param_arry[ADDR_RPS_SET_H] = 0x00;
param_arry[ADDR_RPS_SET_L] = 0x00;
}
}else{
trq_over_cont = 0;
}
HAL_GPIO_WritePin(TEST_GPIO_Port, TEST_Pin, GPIO_PIN_RESET);
//printf("%5.2f_%5.2f_%5.2f\r\n",P,I,D);
}
if (htim == &htim7){
HAL_GPIO_TogglePin(LED_GPIO_Port, LED_Pin);
//printf("%5.2f_%5.2f_%5.2f_%5.2f\r\n",g_rps_raw,g_rps_ave,g_rps_max,g_rps_min);
//int16_t rps_ave = (int16_t)(((param_arry[ADDR_RPS_AVE_H] << 8) & 0xFF00) | (param_arry[ADDR_RPS_AVE_L] & 0x00FF));
}
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
PWM制御の差異と出力トルク
現在作成中の猫を追いかける予定の車ですが、
概ねモーター制御側の雛形は完成してきました。
最終的にバッテリーで長時間駆動させるために、
適切なモーター駆動条件を確認しました。
確認した項目としては、以下の通り。
・PWM駆動周波数
・PWMOFF期間をFastDecay、SlowDecay
・減速側出力のON/OFF
■HブリッジのFastDecayとSlowDecayについて
概要だけ説明します。
FastDecay:PWMOFF期間を電源側へ回生
SlowDecay:ダイオードを介して逆起電力を再利用。
※SlowDecayは回生ブレーキと同位になります。
IndexPro様に分かりやすい解説があるので、紹介します。
尚、L298Nを使用する場合、
SlowDecay:EnablePinをONしたまま、InputPinをPWM制御
FastDecay:InputPinをCWかCCWで固定し、EnablePinをPWM制御
データシート抜粋し以下の通りです。
■試験条件
機材詳細は以下の通り。
但し、以前は各種モータードライバの駆動はフォトカプラを介して駆動していたが
■試験方法
以下写真のように、駆動輪を空転させる形で実施。
・マイコン内で5ms周期でPID制御を実施。
・回転数が規定回転数に到達後、指令値±1rpm時点のPWMのDuty平均値
※回転数、及びDutyは5msサンプリングの50回移動平均値
・駆動電源は、DC12V/1.5Aスイッチング電源
■実験1
PID制御で加速側のみCW方向にPWM出力ON、減速側はDuty0%。
PWMOFF期間をFastDecay、SlowDecay
PWM駆動周波数は50Hz~100kHz
■実験2
実験結果1に付随して
・PID制御の減速側CCW出力有無で比較。
尚、駆動周波数は代表として10kHzで実施。
※CW,CCWの総和より時間平均としてある。
■実験結果まとめ
・基本的にFastDecayを採用するメリットはない。
・スイッチング損失等色々コミコミで考えたときに、SlowDecay運用であれば
PWM駆動周波数1kHz以上は、ほぼ出力トルクは一定と考えられる。
・定速状態であれば、PID出力の減速側をCCW出力を行ったとしても
概ね5%程度でDutyが増加した。
■所感
特に細かく制御を切り替えず
PWM駆動周波数は10k~50kHz程度で、SlowDecacy運用。
急停止による駆動系への負荷を気にしなければ
終始加減速を考慮したPID出力で良さそう。