Control with VESC driver
We describe the VESC control method in ROS using the VESC driver developed by Michael T. Boulet at MIT.
1. Use Custom App
VESC๋ ๊ธฐ๋ณธ์ ์ผ๋ก Custom App์ด๋ผ๋ ๋ถ๋ถ์ ๋ง๋ค์ด ์ด ๋ถ๋ถ์ด ์ฌ์ฉ์๊ฐ ์ถ๊ฐ์ ์ผ๋ก ๊ฐ๋ฐํ ํ๋ก๊ทธ๋จ์ ์ฝ๊ฒ ํ์ ํ ์ ์๋๋ก ๋ฐฐ๋ คํ์๋ค. OpenRobot์์ ๊ฐ๋ฐํ Custom App์ VESC ๊ธฐ๋ณธ ํ์จ์ด์ application ํด๋์ openrobot ํด๋์ ์ ์ฅํด๋์๋ค.
๊ธฐ๋ณธ Firmware์ app_custm.c ๋ผ๋ ํ์ผ์ด custom app์ ๊ธฐ๋ณธ ํ ํ๋ฆฟ์ด๋ผ๊ณ ํ ์ ์๋๋ฐ, ์ด ๊ณณ์ ์๋ ํ๋ก๊ทธ๋จ ์์ ๋ง์ ๋ณด๊ณ ๋ก์ฒ์๋ถ ์ง์ custom app์ ๊ฐ๋ฐํ๋ ๊ฒ์ ์ฌ์ด ์ผ์ด ์๋๋ค. ๊ธฐ๋ณธ์ ์ผ๋ก VESC ํ์จ์ด์ ๋์์๋ฆฌ์ ๋ํด ๋จผ์ ์ดํดํ ํ์๊ฐ ์๋ค.
VESC์๋ ์ฌ๋ฌ๊ฐ์ง ํต์ ์ฑ๋๋ค์ด ์๊ณ ๋ํ์ ์ผ๋ก UART, USB ํต์ ์ด VESC์ ๋ช ๋ น์ ๋ด๋ฆด ๋ ์ฌ์ฉ๋๋ค. ์ฌ๊ธฐ์๋ VESC์ ํต์ Protocol์ ๋ํด ๋จผ์ ์ค๋ช ํ๊ฒ ๋ค.
1. VESC ํต์ Protocol
VESC์ ํต์ ํ๋กํ ์ฝ์ ๊ธฐ๋ณธ์ ์ผ๋ก ๋ค์๊ณผ ๊ฐ์ ๊ตฌ์กฐ๋ฅผ ํ๊ณ ์๋ค. ๋ฐ์ดํฐ๋ Packet ๋จ์๋ก ์ ์ก๋๋ฉฐ Packet์ ์ ์กํ๊ณ ์ ํ๋ Data ์ ์ฌ์ด์ฆ์ ๋ฐ๋ผ Short Packet ๊ณผ Long Packet์ ๋ ๊ฐ์ง ์ข ๋ฅ๊ฐ ์๋ค.
Short Packet
1st Byte
2nd Byte
3rd ~ (n-3)th Byte
(n-2)th Byte
(n-1)th Byte
(n)th Byte
2
packet length
data
CRC High
CRC Low
3
Short Packet ์ ์ฒซ๋ฒ์งธ Byte ์ซ์ 2๋ก ์์ํ๋ค. ์ดํ 2๋ฒ์งธ Byte์ packet์ ๊ธธ์ด ๊ฐ์ ๋ฃ๋๋ค. 3๋ฒ์งธ Byte๋ถํฐ๊ฐ ์ค์ ๋ฐ์ดํฐ์ด๋ฉฐ ์ ์กํ๋ ๋ฐ์ดํฐ์ ํฌ๊ธฐ์ ๋ฐ๋ผ ์ด ๋ถ๋ถ์ด ์ฐจ์งํ๋ ํฌ๊ธฐ๋ ๋ฐ๋ ์ ์๋ค. ์ดํ ๋ง์ง๋ง์์ 3๋ฒ์งธ์ 2๋ฒ์งธ์ CRC Check sum์ ์ํ 2๊ฐ์ Byte๊ฐ ์ฌ์ฉ๋๊ณ ๋ง์ง๋ง Byte๋ Stop Byte๋ฅผ ๋ํ๋ด๋ฉฐ ์ซ์ 3์ ์ฌ์ฉํ๋ค.
Short Packet ์ Packet์ ํฌ๊ธฐ๋ฅผ ํํํ๋๋ฐ 1Byte๋ฅผ ์ฌ์ฉํ๋ฉฐ 1Byte๋ก ํํํ ์ ์๋ ์ซ์์ ํฌ๊ธฐ๋ 0~255๊น์ง์ด๋ฏ๋ก Short Packet์ ์ค๋ ค์ ์ ์ก๋ ์ ์๋ ์ค์ data์ ํฌ๊ธฐ๋ 255 Byte๊ฐ ์ต๋๋ผ๊ณ ๋ณด๋ฉด ๋๋ค. ์ด๋ ๊ฒ Short Packet์ ์ต๋๋ก ๋ฐ์ดํฐ๋ฅผ ์ค์ด์ ์ ์กํ๊ฒ ๋๋ฉด ์ ์ฒด packet์ ์ฌ์ด์ฆ๋ 1(์์) + 1(length) + 255(data) + 2(crc) + 1(stop) = 260 Byte๋ผ๊ณ ๋ณด๋ฉด ๋๋ค.
์ผ๋ฐ์ ์ผ๋ก ๋ง์ดํฌ๋กํ๋ก์ธ์์์ int32, float๋ฑ์ ๋ฐ์ดํฐ๊ฐ 4byte ๋ฐ์ดํฐํ์ด๋ฏ๋ก ๋จ์ ๊ณ์ฐ์ผ๋ก ์๊ฐํด๋ณด๋ฉด 255 Byte๋ 63๊ฐ์ float ๋ณ์๋ฅผ ์ ์กํ ์ ์๋ ๋ฐ์ดํฐ ์ฌ์ด์ฆ๋ผ๊ณ ๋ณผ ์ ์๋ค.
Long Packet
1st Byte
2nd Byte
3rd Byte
4th ~ (n-3)th Byte
(n-2)th Byte
(n-1)th Byte
(n)th Byte
3
packet
length High
packet
length Low
data
CRC High
CRC Low
3
Long Packet ์ ์ฒซ๋ฒ์งธ Byte๊ฐ ์ซ์ 3์ผ๋ก ์์ํ๊ณ ์ด์ด์ 2~3๋ฒ์งธ Byte๊ฐ packet length์ ํด๋นํ๋ค. Packet length์ 2Byte๋ฅผ ํ ๋นํ๋ฏ๋ก ์ต๋๋ก ์ ์ก๊ฐ๋ฅํ ๋ฐ์ดํฐ์ ์์ 65535 Byte๊ฐ ๋๋ค. ์ด๋ Short Packet์ ๋นํด 257๋ฐฐ ๋ ํฐ ๋ฐ์ดํฐ๋์ ํด๋นํ๋ค. ์ดํ Byte์๋ Short Packet๊ณผ ๋์ผํ๊ฒ CRC์ 2Byte์ Stop Byte๋ก ์ซ์ 3์ด ์ฐ์ธ๋ค.
2. VESC ํต์ Interface
VESC์ ํต์ ํ๋กํ ์ฝ์ ๋ํด์๋ ์์์ ์ด์ผ๊ธฐ ํ์์ผ๋ฉฐ ์ด ๋ถ๋ถ์์๋ VESC ํต์ ์ Interface์ ๋ํด ๋ค๋ฃฌ๋ค. VESC๋ ๋ค์ํ ํต์ ๋ฐฉ์์ ์ง์ํ๋ค. ๋ํ์ ์ผ๋ก UART์ USB ๊ทธ๋ฆฌ๊ณ CANํต์ ์ด ์๋ค.
USB๋ ๊ธฐ๋ณธ์ ์ผ๋ก VESC-Tool์ ์ด์ฉํ ๋ ๊ฐ์ฅ ๋ง์ด ์ฐ์ด๋ ํต์ ๋ฐฉ์์ด๋ฉฐ ROS(Robot Operating System)์ ์ด์ฉํ์ฌ VESC๋ฅผ ์ค์๊ฐ ์ ์ดํ ๋๋ ์ฃผ๋ก USB๊ฐ ์ฌ์ฉ๋๋ค.
UART๋ ๊ธฐ๋ณธ์ ์ผ๋ก USB-Serial Adapter๋ฅผ ์ด์ฉํ๋ฉด USB๋ฅผ ์ด์ฉํ๋ ๊ฒ๊ณผ ๋์ผํ๊ฒ VESC-Tool์ด๋ ROS๋ฑ์ ์ฌ์ฉ ํ ์ ์๋ค. ๋ํ, UART๋ ๋ธ๋ฃจํฌ์ค ๋๊ธ(nRF51822)์ ์ฐ๊ฒฐํ์ฌ ๋ฌด์ ์ผ๋ก ์๋๋ก์ด๋์ฉ VESC-Tool์์ VESC๋ฅผ ๋ชจ๋ํฐ๋ง ๋ฐ ์ ์ดํ ์ ์๋๋ก ํ๋ค.
CAN ํต์ ์ ๋ค์์ VESC๋ฅผ ์๋ก ์ฐ๊ฒฐํ์ฌ ํต์ ํ ์ ์๊ฒ ํ๋ฉฐ ํต์ ๋ฐ์ดํฐ๋ฅผ Forwardingํ์ฌ ํ๋์ VESC๋ก ์ ๋ฌ๋ ๋ฐ์ดํฐ๋ฅผ ๋ค๋ฅธ VESC๋ก ์ ๋ฌํ๋๋ฐ ์ฌ์ฉ๋๋ค.
์ด๋ฐ์๋ I2C ํต์ ์ ์ด์ฉํ์ฌ ๋ฌด์ ์ ์ด๊ธฐ์ธ Nunchuk(๊ณผ๊ฑฐ ๋ํ ๋ Wii ๊ฒ์ ์ปจํธ๋กค๋ฌ๋ก ์ฌ์ฉ๋จ)์ ์ด์ฉํด VESC๋ฅผ ์ ์ดํ ์ ์๋ค. I2C ํต์ ์ IMU์ ๋ฐ์ดํฐ๋ฅผ ์ฝ์ด๋ค์ด๊ธฐ ์ํด์๋ ์ฌ์ฉ๋๋ฉฐ ํ์ฌ VESCular์๋ Bosch ์ฌ์ BMI160 IMU๊ฐ On-board์ ํ์ฌ๋์ด Custom App์ ์ฌ์ฉ์ ๊ธฐ๋ณธ์ ์ผ๋ก IMU ๋ฐ์ดํฐ๋ฅผ ์ฌ์ฉํ ์ ์๋ค.
์ด๋ฐ์๋ SPI ํต์ ์ Magnetic Encoder์ธ AS5047p์ ์ฐ๊ฒฐ์ ์ฌ์ฉํ๋๋ฐ ํน์ดํ ์ ์ ๊ธฐ๋ณธ์ ์ผ๋ก๋ Hardware SPI๊ฐ ์๋ Software SPI๋ฅผ ์ฌ์ฉํ๋ค๋ ๊ฒ(Firmware์์ Hardware SPI๋ฅผ ์ฌ์ฉํ ์ ์๋๋ก ๋ฐ๊ฟ์๋ ์๋ค)์ด๊ณ DRV8301(Mosfet Gate Driver)๊ณผ์ ํต์ ๋ Software SPI ํต์ ์ ์ด์ฉํ๋ค.
Hardware SPI๋ PA4~PA7์ ํ ๋น๋ SPI1 ๋ชจ๋์ ์ฌ์ฉํ ์ ์๋๋ฐ ์ด ํ๋ค์ UART ๋ฐ I2C ํต์ ์ผ๋ก๋ Alternate ํด์ ์ฌ์ฉํ ์ ์๊ธฐ ๋๋ฌธ์ PA4~PA7์ ํ ๋นํ ์ ์๋ ํต์ ๊ธฐ๋ฅ์ UART, I2C, SPI ์ธ๊ฐ์ง ํต์ ์ค ๋์์ ํ๋๋ง ์ฌ์ฉ ๊ฐ๋ฅํ๋ค.
OpenRobot App์์๋ Hardware SPI ํต์ ์ ์ด์ฉํ์ฌ Arduino์ ๊ณ ์ ํต์ ์ด ๊ฐ๋ฅํ๋๋ก ํ์ฌ VESC์ ๋ํ ์์ธํ ์ฌํญ์ ๋ชฐ๋ผ๋ Arduino์์ SPI ํต์ ์ ํตํด ์ ์ด๊ฐ ๊ฐ๋ฅํ๋๋ก ํ๋ก๊ทธ๋จํ์๋ค. Hardware SPI ํต์ ๊ธฐ๋ฅ์ OpenRobot App์ ๋ ์์ ์ธ ๊ธฐ๋ฅ์ด๋ค.
์๋ ํ์ VESC์ ํต์ Interface๋ฅผ ๋ชจ๋ ์ ๋ฆฌํ๋ฉด ๋ค์๊ณผ ๊ฐ๋ค.
ํต์ ์ฑ๋
์ฌ์ฉ
ํต์ ์
USB
VESC-Tool, ROS
๊ธฐ๋ณธ 115200bps, ์ต๋ 3000000bps
UART
VESC-Tool, ROS, Bluetooth
๊ธฐ๋ณธ 115200bps, ์ต๋ 921600bps
CAN
VESC to VESC
๊ธฐ๋ณธ 500Kbps, ์ต๋ 1Mbps
I2C (Software)
Nunchuk, IMU
๊ธฐ๋ณธ 1Mbit/sec, ์ต๋ 2Mbit/sec
SPI (Software)
DRV8301, AS5047p
Bit-banging (didn't measure yet)
SPI (Hardware)
Arduino (OpenRobot App Only)
๊ธฐ๋ณธ 4Mhz, ์ต๋ 8Mhz(Clock Speed)
VESC ๊ธฐ๋ณธ ํ์จ์ด์์๋ I2C ๋ฐ SPI ํต์ ์ Software ์ ์ธ ๋ฐฉ์์ผ๋ก ๊ตฌํํ์๋๋ฐ, ์ด๋ STM32F4x์์ ์ ๊ณตํ๋ ํน์ ํ๋ง๋ค ํ ๋น๋ ํ๋์จ์ด์ ์ธ ํต์ Pin์ ์ฌ์ฉํ์ฌ ๊ตฌํํ๋ ๋ฐฉ์์ด ์๋, ์ผ๋ฐ์ ์ธ digital IO๋ฅผ ์ด์ฉํ์ฌ ํต์ ์ ๊ตฌํํ ๊ฒ์ผ๋ก ์๋์ด๋ ธ ์ฐ์ software serial ๊ณผ ์ ์ฌํ ๋ฐฉ์์ผ๋ก ๋ณด๋ฉด ๋๋ค.
์ด๋ฐ ๋ฐฉ์์ผ๋ก ๊ตฌํํ ๋ฐ๋ ์๋ฌด๋๋ ๋ชจํฐ ์ ์ด์ ์ข๋ ํ๋์จ์ด ๋ฆฌ์์ค๋ฅผ ๋ ํฌ์ ํ๊ณ ์ ํ๋ ์๋๊ฐ ๋ณด์ธ๋ค. Hardware ํต์ ์ฑ๋์ ์ด์ฉํ๊ฒ ๋๋ฉด ์๋ฌด๋๋ ํต์ ์ Hardware ์ธํฐ๋ฝํธ๊ฐ ๋ฐ์ํ ์ ์๊ณ ์ด ์ํฉ์์ ๋ชจํฐ์ ์ด๋ฅผ ์ํ Interrupt ๋ฑ๊ณผ ์ซ๋ ์ ์์ด์ ํ์จ์ด๊ฐ ๋ฉ์ถฐ๋ฒ๋ฆฌ๊ฑฐ๋ ๋ฆฌ์ ๋๋ ๋ฌธ์ ๋ฑ์ด ๋ฐ์ํ ์ํ์์ง๊ฐ ์๋ค. ๋ฌผ๋ก ํ๋ก๊ทธ๋จ์ ์ ๋ง๋ค๋ฉด ์ด๋ฐ ์ํฉ์ ํผํ ์๋ ์๊ฒ ์ง๋ง, software ์ ์ผ๋ก๋ ์ถฉ๋ถํ ์ํํ ํต์ ๊ตฌํ์ด ๊ฐ๋ฅํ๋ค๋ฉด ๊ตณ์ด Hardware ๋ฐฉ์์ ๊ฐ ํ์๋ ์๋ค.
๊ตณ์ด Hardware ํต์ ๋ฐฉ์์ ์จ์ผํ ๋๋ ๊ณ ์์ ํต์ ์ด ์๊ตฌ๋ ๋์ด๋ฉฐ, VESCuino์ ๊ฒฝ์ฐ์๋ Hardware SPIํต์ ์ ์ด์ฉํด ์๋์ด๋ ธ Due์ ์ต๋ 8Mhz SPI Clock ์คํผ๋๋ก ํต์ ์ด ๊ฐ๋ฅํ์ฌ ๊ณ ์์ ์ค์๊ฐ ์ ์ด(Low Latency)์ ์ ๋ฆฌํ๋ค.
3. VESC์ ํต์ Software Architecture
์์์ ์ธ๊ธํ ๋ค์ํ ํต์ ์ฑ๋์ ๋นํด ๋ฐ์ดํฐ๋ฅผ ์ฒ๋ฆฌํ๋ software ๋ฃจํด์ ๋งค์ฐ ๊ฐ๋จํ๊ฒ ๊ตฌ์ฑ๋์ด ์๋ค. ์ด๋ ๋ชจ๋ ํต์ ์ฑ๋์์ ๋ค์ด์ค๋ ๋ฐ์ดํฐ๋ฅผ ํ์ฑํ์ฌ ์ค์ data๋ง ์ถ์ถํ ํ ์ด๋ฅผ ํด์ํ๋ ๊ตฌ๋ฌธ์ด ํ ๊ณณ์์ ๋์ผํ๊ฒ ์๋ํ๋๋ก ์ค๊ณํด๋์๊ธฐ ๋๋ฌธ์ด๋ค.
VESC ํ์จ์ด์์๋ C์ธ์ด๋ก ์ด๋ฅผ ๊ตฌํํ๊ธฐ ์ํด ํจ์ ํฌ์ธํฐ๋ฅผ ์ด์ฉํ์ฌ ํต์ ์ฑ๋๋ง๋ค ๋ฐ์ดํฐ ํ์ฑ์ ์ํ ํจ์๋ฅผ ์ํฉ์ ๋ฐ๋ผ ๋ฐ๊ฟ์ค ์ ์๋๋ก ๊ตฌ์ฑ์ด ๋์ด์๋ค. ํจ์ ํฌ์ธํฐ๋ผ๋ ๊ฒ์ด ์กฐ๊ธ ์์ํ ์ ์๋ ๋ถ๋ถ์ธ๋ฐ ์ด๋ ๋ค์๊ณผ ๊ฐ์ด ์ค๋ช ํ ์ ์๋ค.
UART ํต์
VESC-Tool์ ์คํํด๋ณด๋ฉด ํฌ๊ฒ ๋๊ฐ์ ํํธ๋ก ๋๋์ด ์๋ ๊ฒ์ ์์ ์๋๋ฐ ๋ฐ๋ก Motor setting ๋ถ๋ถ๊ณผ App setting ๋ถ๋ถ ๊ทธ๋ฆฌ๊ณ Data Analysis ์ด๋ค. Motor setting์ ๋ชจํฐ์ Identification ๋ฐ ์ ์ดGain๋ฑ์ ์ธํ ๊ณผ ๊ด๋ จ๋ ๋ถ๋ถ์ด๊ณ App setting์ ์ด๋ ํ App์ ์คํ์ํฌ๊ฑด์ง ๊ทธ๋ฆฌ๊ณ ํน์ App ์ด ์คํ์ค์ผ๋ ์ด์ ๊ดํ ์ธํ ์ ๋ํ ๋ถ๋ถ์ด๋ค. ์ธํ ๊ณผ ๊ด๋ จ๋ ๋ ๋ถ๋ถ์ ๊ฐ๊ธฐ ๋ถ๋ฆฌ๋ ์ ์ฅ์์ญ์ ์ฌ์ฉํ์ฌ ๋ ๋ฆฝ์ ์ผ๋ก ๋ณ๊ฒฝํ๊ณ ์ ์ฅ์ํค๋๋ก ๋์ด์๋ค. ํ์ฌ VESC-Tool์์ ์ ํ๊ฐ๋ฅํ App์ ๋ค์๊ณผ ๊ฐ๋ค(VESC FW ver5.02 ๊ธฐ์ค).
No App
PPM
ADC
UART
PPM and UART
ADC and UART
Nunchuk (I2C, Nyko Kama)
NRF
Custom User App
Balance
PAS
ADC and PAS
์์์ ์ธ๊ธํ ๋ฐ์ ๊ฐ์ด VESC์์ ํต์ ์ ์ํด ์ฌ์ฉํ๋ ์ปค๋ฅํฐ๋ ์ฌ๋ฌ ํต์ ์ด ํตํฉ๋์ด ์์ด ํ๋ฒ์ ์ฌ๋ฌ๊ฐ์ง ์ญํ ์ ์ํํ ์๋ ์๋ค. ๊ทธ๋์ ์์ ๊ฐ์ด App์ ์ ํํ์ฌ ํต์ ํ์ด ์ ์์ธ๊ฐํ ์ด๊ธฐํ๋ ๋ ์ํ๋ ํต์ ํ์ ์ญํ ์ ์ํํ ์ ์๋๋ก ์ธํ ๋๋ค.
UARTํต์ ์ ์ํด์๋ Tx, Rx (Tx๋ ์ก์ , Rx๋ ์์ )๋ฅผ ์ํ Pin์ด ์๊ตฌ๋๋ฉฐ ์ด๋ฅผ ๋ฐ๋ํธ ํต์ ์ธก๊ณผ ์๋์ ๊ฐ์ด ์ฐ๊ฒฐํ๋ฉด UARTํต์ ์ด ๊ฐ๋ฅํ๋ค. UART ํต์ ์ ๋ฐ๋์ GND๋ฅผ ๋์ผํ๊ฒ ํด์ค์ผํ๋ฏ๋ก ์ค์ง์ ์ผ๋ก๋ 3๊ฐ์ ์ ์ด ์๋ก ์ฐ๊ฒฐ๋์ด์ผ ํ๋ค.
VESC ์ชฝ TX - ๋ฐ๋์ชฝ RX
VESC ์ชฝ RX - ๋ฐ๋์ชฝ TX
VESC ์ชฝ GND - ๋ฐ๋์ชฝ GND
UART ํต์ ๊ดํด์๋ ๊ธฐ๋ณธ์ ์ผ๋ก ์๋์ ํ๋ก๊ทธ๋จ์ ์ดํด๋ณด์์ผ ํ๋ค.
app_uartcomm.c
packet.c
commands.c
์ ํ๋ก๊ทธ๋จ์ ๊ตฌ์กฐ๋ฅผ ์ ์ดํดํ๋ฉด ์ ์ฒด์ ์ธ VESC์ ํต์ ๊ตฌ์กฐ๋ฅผ ์ดํดํ ์ ์๊ฒ๋๋๋ฐ, ์ฐ์ app_uartcomm.c์์๋ app_uartcomm_start()ํจ์์์ UART ํต์ ์ ์ํ Pin ์ค์ ๊ณผ UART ํต์ ์ ์ฒ๋ฆฌํ๋ Thread๋ฅผ ์คํ์ํจ๋ค. ๊ทธ๋ฆฌ๊ณ ๊ทธ ์ด์ ์ packet_init ํจ์์์ UART ํต์ ์ ๋ฐ์ดํฐ Packet์ ์ฒ๋ฆฌํ ํจ์๋ค์ ์ ํํด์ฃผ ํจ์ํฌ์ธํฐ๋ฅผ ์ง์ ํ๋ค.
void app_uartcomm_start(void) {
// UART ํต์ Packet์ ์ฒ๋ฆฌํด์ฃผ๋ ํจ์๋ฅผ ์ง์ ํ๊ธฐ ์ํ ํจ์ํฌ์ธํฐ
packet_init(send_packet, process_packet, PACKET_HANDLER);
// UART ํต์ ์ฒ๋ฆฌ Thread
if (!thread_is_running) {
chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
NORMALPRIO, packet_process_thread, NULL);
thread_is_running = true;
}
// UART Pin ์ค์ ๋ถ
sdStart(&HW_UART_DEV, &uart_cfg);
palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_PULLUP);
palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
PAL_STM32_OSPEED_HIGHEST |
PAL_STM32_PUDR_PULLUP);
uart_is_running = true;
}
๋ฐ๋ก ์ด packet_init ํจ์๊ฐ ๋งค์ฐ ์ค์ํ๋ค. packet_init ํจ์์ ์ํ์ ๋ณด๋ฉด ๋ค์๊ณผ ๊ฐ๋ค. packet.c์ ๊ฐ๋ณด๋ฉด ์๋์ ๊ฐ์ด ๋์ด์๋ค. packet_init ํจ์์ ์ฒซ๋ฒ์งธ input์๋ send_func(์ก๋ฅผ ์ํ ํจ์)์ ํจ์ํฌ์ธํฐ๋ฅผ ์ง์ ํ ์ ์๊ณ ๋๋ฒ์งธ input์๋ process_func(์์ ์ ์ํ ํจ์)์ ํจ์ํฌ์ธํฐ๋ฅผ ์ง์ ํ ์ ์๋ค.
void packet_init(void (*s_func)(unsigned char *data, unsigned int len),
void (*p_func)(unsigned char *data, unsigned int len), int handler_num) {
memset(&m_handler_states[handler_num], 0, sizeof(PACKET_STATE_t));
m_handler_states[handler_num].send_func = s_func;
m_handler_states[handler_num].process_func = p_func;
๊ทธ๋ผ ์์์ UART ํต์ ์ ๊ฒฝ์ฐ๋ฅผ ์ดํด๋ณด๋ฉด packet_init ํจ์์ ์ฒซ๋ฒ์งธ input์ผ๋ก ์ง์ ํ send_packet ํจ์์ ๋ด์ฉ์ ๋ค์๊ณผ ๊ฐ๋ค.
static void send_packet(unsigned char *data, unsigned int len) {
if (uart_is_running) {
sdWrite(&HW_UART_DEV, data, len);
}
}
send_packet ํจ์๋ app_uartcomm.c ํ์ผ์์ ์์ผ๋ฉฐ ๋จ์์ด data์ data์ ๊ธธ์ด(len)๋ฅผ ์๋ฉด ์ด๋ฅผ UART ํต์ ์ผ๋ก ์ ์กํ๋ ์๋ฌด๋ฅผ ์ํํ๋ค.
packet_init ํจ์์ ๋๋ฒ์งธ input์ธ process_packet ํจ์๋ ๋ค์๊ณผ ๊ฐ๋ค. process_packet ํจ์๋ app_uartcomm.c ํ์ผ์์ ์์ผ๋ฉฐ data์ data์ ๊ธธ์ด(len)์ commands_process_packet ํจ์์ ์ ๋ฌํ๋๋ก ๋์ด์์ผ๋ฉฐ commands_process_packet ํจ์๋ commands.c ํ์ผ์ ์์ฑ๋์ด ์๋ค.
static void process_packet(unsigned char *data, unsigned int len) {
commands_process_packet(data, len, app_uartcomm_send_packet);
}
์ฌ๊ธฐ์ commands_process_packet ํจ์์ ์ธ๋ฒ์งธ input์ผ๋ก ์ง์ ๋ app_uartcomm_send_packet ํจ์๋ ์ผ๋ช reply ํจ์๋ก UARTํต์ ์ ํตํด ์ฐ๊ฒฐ๋ ์ฅ์น์์ ์ ๋ฌํ ๋ช ๋ น์ ๋ํ ๋ต๋ณ์ ํ ๋ ์ฌ์ฉ๋๋ ํจ์์ด๋ค. ์์์ send_packet ํจ์์ ๋ค๋ฅธ ์ ์ send_packet ํจ์๋ VESC ์ธก์์ ๋จผ์ ๋ฐ์ดํฐ Packet์ ์ ์กํ ๋ ์ฌ์ฉ๋๋ ํจ์์ด๊ณ app_uartcomm_send_packet ํจ์๋ UARTํต์ ์ผ๋ก ์ฐ๊ฒฐ๋ ๋ฐ๋์ชฝ ์ฅ์น์์ ๋จผ์ ๋ฐ์ดํฐ Packet์ด ์ ์ก๋์ด ์ด์ ๋ํ ๋ต๋ณ์ ์ํ ์ฉ๋๋ก ์ฌ์ฉ๋๋ค๊ณ ์๊ฐํ๋ฉด ๋๋ค.
๊ทธ๋ผ ์ค์ง์ ์ผ๋ก Data์ ํ๋ฆ์ด ์ด๋ป๊ฒ ์ด๋ค์ง๋์ง ์ข ๋ ์์ธํ๊ฒ ์์๋ณด์
Last updated
Was this helpful?