Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 0b22368e6f | |||
| 1aacb07132 |
@@ -0,0 +1,6 @@
|
||||
#ifndef APP2_CMD_SRV_H
|
||||
#define APP2_CMD_SRV_H
|
||||
|
||||
void cmd_srv_process(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,27 @@
|
||||
#ifndef APP2_HAL_DRV_H
|
||||
#define APP2_HAL_DRV_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#define APP2_UART_RX_BUF_SIZE 128u
|
||||
|
||||
void hal_init(void);
|
||||
|
||||
/* Producer-side UART RX APIs */
|
||||
void hal_uart_rx_isr_push_char(char ch);
|
||||
bool hal_uart_is_frame_ready(void);
|
||||
const volatile char *hal_uart_get_rx_frame(void);
|
||||
void hal_uart_clear_rx_frame(void);
|
||||
|
||||
/* Consumer-side TX and peripheral APIs */
|
||||
void hal_uart_send_str(const char *str);
|
||||
bool hal_spi_send_bytes(const uint8_t *data, uint16_t len);
|
||||
uint16_t hal_adc_get_latest_mv(void);
|
||||
int32_t hal_speed_get_feedback_rpm(void);
|
||||
|
||||
/* Time base used by services */
|
||||
uint32_t hal_get_tick_ms(void);
|
||||
void hal_tick_inc_1ms(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,29 @@
|
||||
#ifndef APP2_MOTOR_CTRL_CORE_H
|
||||
#define APP2_MOTOR_CTRL_CORE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int32_t target_speed_rpm;
|
||||
int32_t current_speed_rpm;
|
||||
int32_t target_amp_permille;
|
||||
int32_t current_amp_permille;
|
||||
uint8_t fault_state;
|
||||
bool motor_enable;
|
||||
bool regular_data_enable;
|
||||
uint16_t regular_data_period_ms;
|
||||
} Motor_Ctrl_t;
|
||||
|
||||
extern Motor_Ctrl_t g_motor_ctrl;
|
||||
|
||||
void motor_ctrl_init(void);
|
||||
void motor_ctrl_tick_1ms(void);
|
||||
|
||||
bool motor_ctrl_set_target_speed(int32_t rpm);
|
||||
bool motor_ctrl_set_target_amp(int32_t permille);
|
||||
void motor_ctrl_set_regular_data_enable(bool en);
|
||||
void motor_ctrl_set_regular_period_ms(uint16_t period_ms);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,6 @@
|
||||
#ifndef APP2_STREAM_SRV_H
|
||||
#define APP2_STREAM_SRV_H
|
||||
|
||||
void stream_srv_process(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,124 @@
|
||||
#include "cmd_srv.h"
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "hal_drv.h"
|
||||
#include "motor_ctrl_core.h"
|
||||
|
||||
static int cmd_strcmp(const char *buf, const char *cmd)
|
||||
{
|
||||
while (*cmd)
|
||||
{
|
||||
if (*buf != *cmd)
|
||||
return 1;
|
||||
buf++;
|
||||
cmd++;
|
||||
}
|
||||
return (*buf != '\0');
|
||||
}
|
||||
|
||||
static bool cmd_match_with_arg(const char *buf, const char *prefix, const char **arg_out)
|
||||
{
|
||||
size_t len = strlen(prefix);
|
||||
for (size_t i = 0; i < len; i++)
|
||||
{
|
||||
if (buf[i] != prefix[i])
|
||||
return false;
|
||||
}
|
||||
|
||||
if (buf[len] == ' ')
|
||||
{
|
||||
*arg_out = &buf[len + 1];
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void cmd_srv_process(void)
|
||||
{
|
||||
char cmd_line[APP2_UART_RX_BUF_SIZE] = { 0 };
|
||||
uint16_t i = 0u;
|
||||
const char *arg = NULL;
|
||||
|
||||
if (!hal_uart_is_frame_ready())
|
||||
return;
|
||||
|
||||
{
|
||||
const volatile char *rx = hal_uart_get_rx_frame();
|
||||
while ((i < (APP2_UART_RX_BUF_SIZE - 1u)) && (rx[i] != '\0'))
|
||||
{
|
||||
cmd_line[i] = (char)rx[i];
|
||||
i++;
|
||||
}
|
||||
cmd_line[i] = '\0';
|
||||
}
|
||||
|
||||
if (cmd_strcmp(cmd_line, "help") == 0)
|
||||
{
|
||||
hal_uart_send_str("help\r\n");
|
||||
hal_uart_send_str(" spd_set <rpm>\r\n");
|
||||
hal_uart_send_str(" amp_set <0..1000>\r\n");
|
||||
hal_uart_send_str(" regular_data_en 1/0\r\n");
|
||||
hal_uart_send_str(" regular_data_period <ms>\r\n");
|
||||
hal_uart_send_str(" motor_get\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "spd_set", &arg))
|
||||
{
|
||||
int32_t rpm = (int32_t)strtol(arg, NULL, 10);
|
||||
(void)motor_ctrl_set_target_speed(rpm);
|
||||
hal_uart_send_str("OK spd_set\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "amp_set", &arg))
|
||||
{
|
||||
int32_t amp = (int32_t)strtol(arg, NULL, 10);
|
||||
(void)motor_ctrl_set_target_amp(amp);
|
||||
hal_uart_send_str("OK amp_set\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "regular_data_en", &arg))
|
||||
{
|
||||
if (arg[0] == '1')
|
||||
{
|
||||
motor_ctrl_set_regular_data_enable(true);
|
||||
hal_uart_send_str("OK regular_data on\r\n");
|
||||
}
|
||||
else if (arg[0] == '0')
|
||||
{
|
||||
motor_ctrl_set_regular_data_enable(false);
|
||||
hal_uart_send_str("OK regular_data off\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
hal_uart_send_str("ERR usage: regular_data_en 1/0\r\n");
|
||||
}
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "regular_data_period", &arg))
|
||||
{
|
||||
uint16_t ms = (uint16_t)strtoul(arg, NULL, 10);
|
||||
motor_ctrl_set_regular_period_ms(ms);
|
||||
hal_uart_send_str("OK regular_data_period\r\n");
|
||||
}
|
||||
else if (cmd_strcmp(cmd_line, "motor_get") == 0)
|
||||
{
|
||||
char out[96] = { 0 };
|
||||
snprintf(out,
|
||||
sizeof(out),
|
||||
"target_spd=%ld cur_spd=%ld target_amp=%ld cur_amp=%ld fault=%u stream=%u\r\n",
|
||||
(long)g_motor_ctrl.target_speed_rpm,
|
||||
(long)g_motor_ctrl.current_speed_rpm,
|
||||
(long)g_motor_ctrl.target_amp_permille,
|
||||
(long)g_motor_ctrl.current_amp_permille,
|
||||
(unsigned)g_motor_ctrl.fault_state,
|
||||
(unsigned)g_motor_ctrl.regular_data_enable);
|
||||
hal_uart_send_str(out);
|
||||
}
|
||||
else
|
||||
{
|
||||
hal_uart_send_str("ERR unknown cmd\r\n");
|
||||
}
|
||||
|
||||
hal_uart_clear_rx_frame();
|
||||
}
|
||||
@@ -0,0 +1,86 @@
|
||||
#include "hal_drv.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
static volatile char s_uart_rx_buf[APP2_UART_RX_BUF_SIZE];
|
||||
static volatile uint16_t s_uart_rx_idx;
|
||||
static volatile bool s_uart_frame_ready;
|
||||
static uint32_t s_tick_ms;
|
||||
|
||||
void hal_init(void)
|
||||
{
|
||||
s_uart_rx_idx = 0u;
|
||||
s_uart_rx_buf[0] = '\0';
|
||||
s_uart_frame_ready = false;
|
||||
s_tick_ms = 0u;
|
||||
}
|
||||
|
||||
void hal_uart_rx_isr_push_char(char ch)
|
||||
{
|
||||
if (s_uart_frame_ready)
|
||||
return;
|
||||
|
||||
if ((ch == '\r') || (ch == '\n'))
|
||||
{
|
||||
if (s_uart_rx_idx > 0u)
|
||||
{
|
||||
s_uart_rx_buf[s_uart_rx_idx] = '\0';
|
||||
s_uart_frame_ready = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (s_uart_rx_idx < (APP2_UART_RX_BUF_SIZE - 1u))
|
||||
{
|
||||
s_uart_rx_buf[s_uart_rx_idx++] = ch;
|
||||
}
|
||||
}
|
||||
|
||||
bool hal_uart_is_frame_ready(void)
|
||||
{
|
||||
return s_uart_frame_ready;
|
||||
}
|
||||
|
||||
const volatile char *hal_uart_get_rx_frame(void)
|
||||
{
|
||||
return s_uart_rx_buf;
|
||||
}
|
||||
|
||||
void hal_uart_clear_rx_frame(void)
|
||||
{
|
||||
s_uart_rx_idx = 0u;
|
||||
s_uart_rx_buf[0] = '\0';
|
||||
s_uart_frame_ready = false;
|
||||
}
|
||||
|
||||
void hal_uart_send_str(const char *str)
|
||||
{
|
||||
(void)printf("%s", str);
|
||||
}
|
||||
|
||||
bool hal_spi_send_bytes(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
(void)data;
|
||||
(void)len;
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t hal_adc_get_latest_mv(void)
|
||||
{
|
||||
return 12000u;
|
||||
}
|
||||
|
||||
int32_t hal_speed_get_feedback_rpm(void)
|
||||
{
|
||||
return 1000;
|
||||
}
|
||||
|
||||
uint32_t hal_get_tick_ms(void)
|
||||
{
|
||||
return s_tick_ms;
|
||||
}
|
||||
|
||||
void hal_tick_inc_1ms(void)
|
||||
{
|
||||
s_tick_ms++;
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
#include "cmd_srv.h"
|
||||
#include "hal_drv.h"
|
||||
#include "motor_ctrl_core.h"
|
||||
#include "stream_srv.h"
|
||||
|
||||
int main(void)
|
||||
{
|
||||
hal_init();
|
||||
motor_ctrl_init();
|
||||
|
||||
hal_uart_send_str("app2 demo start\r\n");
|
||||
|
||||
while (1)
|
||||
{
|
||||
hal_tick_inc_1ms();
|
||||
|
||||
/* Parse command frames from HAL RX producer */
|
||||
cmd_srv_process();
|
||||
|
||||
/* Run motor control law at fixed rate */
|
||||
motor_ctrl_tick_1ms();
|
||||
|
||||
/* Stream telemetry when regular_data_en is on */
|
||||
stream_srv_process();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,64 @@
|
||||
#include "motor_ctrl_core.h"
|
||||
|
||||
#include "hal_drv.h"
|
||||
|
||||
Motor_Ctrl_t g_motor_ctrl;
|
||||
|
||||
static int32_t clamp_i32(int32_t x, int32_t lo, int32_t hi)
|
||||
{
|
||||
if (x < lo)
|
||||
return lo;
|
||||
if (x > hi)
|
||||
return hi;
|
||||
return x;
|
||||
}
|
||||
|
||||
void motor_ctrl_init(void)
|
||||
{
|
||||
g_motor_ctrl.target_speed_rpm = 0;
|
||||
g_motor_ctrl.current_speed_rpm = 0;
|
||||
g_motor_ctrl.target_amp_permille = 0;
|
||||
g_motor_ctrl.current_amp_permille = 0;
|
||||
g_motor_ctrl.fault_state = 0u;
|
||||
g_motor_ctrl.motor_enable = false;
|
||||
g_motor_ctrl.regular_data_enable = false;
|
||||
g_motor_ctrl.regular_data_period_ms = 20u;
|
||||
}
|
||||
|
||||
bool motor_ctrl_set_target_speed(int32_t rpm)
|
||||
{
|
||||
g_motor_ctrl.target_speed_rpm = clamp_i32(rpm, -6000, 6000);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool motor_ctrl_set_target_amp(int32_t permille)
|
||||
{
|
||||
g_motor_ctrl.target_amp_permille = clamp_i32(permille, 0, 1000);
|
||||
return true;
|
||||
}
|
||||
|
||||
void motor_ctrl_set_regular_data_enable(bool en)
|
||||
{
|
||||
g_motor_ctrl.regular_data_enable = en;
|
||||
}
|
||||
|
||||
void motor_ctrl_set_regular_period_ms(uint16_t period_ms)
|
||||
{
|
||||
if (period_ms == 0u)
|
||||
period_ms = 1u;
|
||||
g_motor_ctrl.regular_data_period_ms = period_ms;
|
||||
}
|
||||
|
||||
void motor_ctrl_tick_1ms(void)
|
||||
{
|
||||
int32_t speed_fb = hal_speed_get_feedback_rpm();
|
||||
|
||||
g_motor_ctrl.current_speed_rpm = speed_fb;
|
||||
|
||||
if (g_motor_ctrl.current_amp_permille < g_motor_ctrl.target_amp_permille)
|
||||
g_motor_ctrl.current_amp_permille++;
|
||||
else if (g_motor_ctrl.current_amp_permille > g_motor_ctrl.target_amp_permille)
|
||||
g_motor_ctrl.current_amp_permille--;
|
||||
|
||||
g_motor_ctrl.fault_state = 0u;
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
#include "stream_srv.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "hal_drv.h"
|
||||
#include "motor_ctrl_core.h"
|
||||
|
||||
static uint32_t s_last_tx_ms;
|
||||
|
||||
static void pack_u16_le(uint8_t *buf, uint16_t v)
|
||||
{
|
||||
buf[0] = (uint8_t)(v & 0xFFu);
|
||||
buf[1] = (uint8_t)((v >> 8) & 0xFFu);
|
||||
}
|
||||
|
||||
static void pack_i32_le(uint8_t *buf, int32_t v)
|
||||
{
|
||||
uint32_t u = (uint32_t)v;
|
||||
buf[0] = (uint8_t)(u & 0xFFu);
|
||||
buf[1] = (uint8_t)((u >> 8) & 0xFFu);
|
||||
buf[2] = (uint8_t)((u >> 16) & 0xFFu);
|
||||
buf[3] = (uint8_t)((u >> 24) & 0xFFu);
|
||||
}
|
||||
|
||||
void stream_srv_process(void)
|
||||
{
|
||||
uint8_t frame[16];
|
||||
uint32_t now;
|
||||
uint16_t bus_mv;
|
||||
|
||||
if (!g_motor_ctrl.regular_data_enable)
|
||||
return;
|
||||
|
||||
now = hal_get_tick_ms();
|
||||
if ((now - s_last_tx_ms) < g_motor_ctrl.regular_data_period_ms)
|
||||
return;
|
||||
|
||||
s_last_tx_ms = now;
|
||||
bus_mv = hal_adc_get_latest_mv();
|
||||
|
||||
frame[0] = 0xA5u;
|
||||
frame[1] = 0x5Au;
|
||||
pack_i32_le(&frame[2], g_motor_ctrl.target_speed_rpm);
|
||||
pack_i32_le(&frame[6], g_motor_ctrl.current_speed_rpm);
|
||||
frame[10] = (uint8_t)g_motor_ctrl.current_amp_permille;
|
||||
frame[11] = g_motor_ctrl.fault_state;
|
||||
pack_u16_le(&frame[12], bus_mv);
|
||||
frame[14] = 0u;
|
||||
frame[15] = 0u;
|
||||
|
||||
(void)hal_spi_send_bytes(frame, (uint16_t)sizeof(frame));
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
#ifndef APP_COPYB_CMD_SRV_H
|
||||
#define APP_COPYB_CMD_SRV_H
|
||||
|
||||
#include "comm_if.h"
|
||||
|
||||
void cmd_srv_process(const CommIf_t *comm);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,27 @@
|
||||
#ifndef APP_COPYB_COMM_IF_H
|
||||
#define APP_COPYB_COMM_IF_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
const uint8_t *data;
|
||||
uint16_t len;
|
||||
} CommFrame_t;
|
||||
|
||||
typedef bool (*CommIsReadyFn)(void *ctx);
|
||||
typedef bool (*CommGetFrameFn)(void *ctx, CommFrame_t *frame);
|
||||
typedef void (*CommClearFrameFn)(void *ctx);
|
||||
typedef bool (*CommTxBytesFn)(void *ctx, const uint8_t *data, uint16_t len);
|
||||
|
||||
typedef struct
|
||||
{
|
||||
void *ctx;
|
||||
CommIsReadyFn is_ready;
|
||||
CommGetFrameFn get_frame;
|
||||
CommClearFrameFn clear_frame;
|
||||
CommTxBytesFn tx_bytes;
|
||||
} CommIf_t;
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,28 @@
|
||||
#ifndef APP_COPYB_MOTOR_CTRL_CORE_H
|
||||
#define APP_COPYB_MOTOR_CTRL_CORE_H
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int32_t target_speed_rpm;
|
||||
int32_t current_speed_rpm;
|
||||
int32_t target_amp_permille;
|
||||
int32_t current_amp_permille;
|
||||
uint8_t fault_state;
|
||||
bool regular_data_en;
|
||||
uint16_t regular_period_ms;
|
||||
} MotorCtrl_t;
|
||||
|
||||
extern MotorCtrl_t g_motor_ctrl;
|
||||
|
||||
void motor_ctrl_init(void);
|
||||
void motor_ctrl_tick_1ms(void);
|
||||
|
||||
bool motor_ctrl_set_speed(int32_t rpm);
|
||||
bool motor_ctrl_set_amp(int32_t amp_permille);
|
||||
void motor_ctrl_set_regular_en(bool en);
|
||||
void motor_ctrl_set_regular_period(uint16_t period_ms);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,11 @@
|
||||
#ifndef APP_COPYB_PLATFORM_STUB_H
|
||||
#define APP_COPYB_PLATFORM_STUB_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
uint32_t platform_tick_ms(void);
|
||||
void platform_tick_inc_1ms(void);
|
||||
uint16_t platform_adc_bus_mv(void);
|
||||
int32_t platform_speed_feedback_rpm(void);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,8 @@
|
||||
#ifndef APP_COPYB_STREAM_SRV_H
|
||||
#define APP_COPYB_STREAM_SRV_H
|
||||
|
||||
#include "comm_if.h"
|
||||
|
||||
void stream_srv_process(const CommIf_t *comm);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef APP_COPYB_TRANSPORT_I2C_H
|
||||
#define APP_COPYB_TRANSPORT_I2C_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "comm_if.h"
|
||||
|
||||
void transport_i2c_init(void);
|
||||
const CommIf_t *transport_i2c_get_if(void);
|
||||
|
||||
/* ISR callback example for packetized I2C slave writes */
|
||||
void transport_i2c_rx_frame_push(const uint8_t *data, uint16_t len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef APP_COPYB_TRANSPORT_SPI_H
|
||||
#define APP_COPYB_TRANSPORT_SPI_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "comm_if.h"
|
||||
|
||||
void transport_spi_init(void);
|
||||
const CommIf_t *transport_spi_get_if(void);
|
||||
|
||||
/* ISR/DMA callback example for frame push in SPI slave mode */
|
||||
void transport_spi_rx_frame_push(const uint8_t *data, uint16_t len);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,14 @@
|
||||
#ifndef APP_COPYB_TRANSPORT_UART_H
|
||||
#define APP_COPYB_TRANSPORT_UART_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "comm_if.h"
|
||||
|
||||
void transport_uart_init(void);
|
||||
const CommIf_t *transport_uart_get_if(void);
|
||||
|
||||
/* ISR entry point example for byte push from UART RX interrupt */
|
||||
void transport_uart_rx_isr_push_byte(uint8_t ch);
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,133 @@
|
||||
#include "cmd_srv.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "motor_ctrl_core.h"
|
||||
|
||||
static int cmd_strcmp(const char *buf, const char *cmd)
|
||||
{
|
||||
while (*cmd)
|
||||
{
|
||||
if (*buf != *cmd)
|
||||
return 1;
|
||||
buf++;
|
||||
cmd++;
|
||||
}
|
||||
return (*buf != '\0');
|
||||
}
|
||||
|
||||
static bool cmd_match_with_arg(const char *buf, const char *prefix, const char **arg_out)
|
||||
{
|
||||
size_t len = strlen(prefix);
|
||||
for (size_t i = 0; i < len; i++)
|
||||
{
|
||||
if (buf[i] != prefix[i])
|
||||
return false;
|
||||
}
|
||||
|
||||
if (buf[len] == ' ')
|
||||
{
|
||||
*arg_out = &buf[len + 1];
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static void comm_send_str(const CommIf_t *comm, const char *str)
|
||||
{
|
||||
(void)comm->tx_bytes(comm->ctx, (const uint8_t *)str, (uint16_t)strlen(str));
|
||||
}
|
||||
|
||||
void cmd_srv_process(const CommIf_t *comm)
|
||||
{
|
||||
char cmd_line[128] = { 0 };
|
||||
CommFrame_t frame = { 0 };
|
||||
uint16_t copy_len;
|
||||
const char *arg = NULL;
|
||||
|
||||
if (!comm->is_ready(comm->ctx))
|
||||
return;
|
||||
|
||||
if (!comm->get_frame(comm->ctx, &frame))
|
||||
{
|
||||
comm->clear_frame(comm->ctx);
|
||||
return;
|
||||
}
|
||||
|
||||
copy_len = frame.len;
|
||||
if (copy_len >= sizeof(cmd_line))
|
||||
copy_len = (uint16_t)(sizeof(cmd_line) - 1u);
|
||||
|
||||
(void)memcpy(cmd_line, frame.data, copy_len);
|
||||
cmd_line[copy_len] = '\0';
|
||||
|
||||
if (cmd_strcmp(cmd_line, "help") == 0)
|
||||
{
|
||||
comm_send_str(comm, "help\r\n");
|
||||
comm_send_str(comm, " spd_set <rpm>\r\n");
|
||||
comm_send_str(comm, " amp_set <0..1000>\r\n");
|
||||
comm_send_str(comm, " regular_data_en 1/0\r\n");
|
||||
comm_send_str(comm, " regular_data_period <ms>\r\n");
|
||||
comm_send_str(comm, " motor_get\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "spd_set", &arg))
|
||||
{
|
||||
int32_t rpm = (int32_t)strtol(arg, NULL, 10);
|
||||
(void)motor_ctrl_set_speed(rpm);
|
||||
comm_send_str(comm, "OK spd_set\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "amp_set", &arg))
|
||||
{
|
||||
int32_t amp = (int32_t)strtol(arg, NULL, 10);
|
||||
(void)motor_ctrl_set_amp(amp);
|
||||
comm_send_str(comm, "OK amp_set\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "regular_data_en", &arg))
|
||||
{
|
||||
if (arg[0] == '1')
|
||||
{
|
||||
motor_ctrl_set_regular_en(true);
|
||||
comm_send_str(comm, "OK regular_data on\r\n");
|
||||
}
|
||||
else if (arg[0] == '0')
|
||||
{
|
||||
motor_ctrl_set_regular_en(false);
|
||||
comm_send_str(comm, "OK regular_data off\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
comm_send_str(comm, "ERR usage: regular_data_en 1/0\r\n");
|
||||
}
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd_line, "regular_data_period", &arg))
|
||||
{
|
||||
uint16_t period = (uint16_t)strtoul(arg, NULL, 10);
|
||||
motor_ctrl_set_regular_period(period);
|
||||
comm_send_str(comm, "OK regular_data_period\r\n");
|
||||
}
|
||||
else if (cmd_strcmp(cmd_line, "motor_get") == 0)
|
||||
{
|
||||
char out[128] = { 0 };
|
||||
(void)snprintf(out,
|
||||
sizeof(out),
|
||||
"target_spd=%ld cur_spd=%ld target_amp=%ld cur_amp=%ld fault=%u stream=%u\r\n",
|
||||
(long)g_motor_ctrl.target_speed_rpm,
|
||||
(long)g_motor_ctrl.current_speed_rpm,
|
||||
(long)g_motor_ctrl.target_amp_permille,
|
||||
(long)g_motor_ctrl.current_amp_permille,
|
||||
(unsigned)g_motor_ctrl.fault_state,
|
||||
(unsigned)g_motor_ctrl.regular_data_en);
|
||||
comm_send_str(comm, out);
|
||||
}
|
||||
else
|
||||
{
|
||||
comm_send_str(comm, "ERR unknown cmd\r\n");
|
||||
}
|
||||
|
||||
comm->clear_frame(comm->ctx);
|
||||
}
|
||||
@@ -0,0 +1,47 @@
|
||||
#include "cmd_srv.h"
|
||||
#include "motor_ctrl_core.h"
|
||||
#include "platform_stub.h"
|
||||
#include "stream_srv.h"
|
||||
#include "transport_i2c.h"
|
||||
#include "transport_spi.h"
|
||||
#include "transport_uart.h"
|
||||
|
||||
#define APP_COPYB_TRANSPORT_UART 1
|
||||
#define APP_COPYB_TRANSPORT_SPI 2
|
||||
#define APP_COPYB_TRANSPORT_I2C 3
|
||||
|
||||
#ifndef APP_COPYB_TRANSPORT_SELECT
|
||||
#define APP_COPYB_TRANSPORT_SELECT APP_COPYB_TRANSPORT_UART
|
||||
#endif
|
||||
|
||||
static const CommIf_t *select_transport(void)
|
||||
{
|
||||
#if APP_COPYB_TRANSPORT_SELECT == APP_COPYB_TRANSPORT_UART
|
||||
transport_uart_init();
|
||||
return transport_uart_get_if();
|
||||
#elif APP_COPYB_TRANSPORT_SELECT == APP_COPYB_TRANSPORT_SPI
|
||||
transport_spi_init();
|
||||
return transport_spi_get_if();
|
||||
#else
|
||||
transport_i2c_init();
|
||||
return transport_i2c_get_if();
|
||||
#endif
|
||||
}
|
||||
|
||||
int main(void)
|
||||
{
|
||||
const CommIf_t *comm = select_transport();
|
||||
|
||||
motor_ctrl_init();
|
||||
|
||||
while (1)
|
||||
{
|
||||
platform_tick_inc_1ms();
|
||||
|
||||
cmd_srv_process(comm);
|
||||
motor_ctrl_tick_1ms();
|
||||
stream_srv_process(comm);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
#include "motor_ctrl_core.h"
|
||||
|
||||
#include "platform_stub.h"
|
||||
|
||||
MotorCtrl_t g_motor_ctrl;
|
||||
|
||||
static int32_t clamp_i32(int32_t value, int32_t min_v, int32_t max_v)
|
||||
{
|
||||
if (value < min_v)
|
||||
return min_v;
|
||||
if (value > max_v)
|
||||
return max_v;
|
||||
return value;
|
||||
}
|
||||
|
||||
void motor_ctrl_init(void)
|
||||
{
|
||||
g_motor_ctrl.target_speed_rpm = 0;
|
||||
g_motor_ctrl.current_speed_rpm = 0;
|
||||
g_motor_ctrl.target_amp_permille = 0;
|
||||
g_motor_ctrl.current_amp_permille = 0;
|
||||
g_motor_ctrl.fault_state = 0u;
|
||||
g_motor_ctrl.regular_data_en = false;
|
||||
g_motor_ctrl.regular_period_ms = 20u;
|
||||
}
|
||||
|
||||
void motor_ctrl_tick_1ms(void)
|
||||
{
|
||||
g_motor_ctrl.current_speed_rpm = platform_speed_feedback_rpm();
|
||||
|
||||
if (g_motor_ctrl.current_amp_permille < g_motor_ctrl.target_amp_permille)
|
||||
g_motor_ctrl.current_amp_permille++;
|
||||
else if (g_motor_ctrl.current_amp_permille > g_motor_ctrl.target_amp_permille)
|
||||
g_motor_ctrl.current_amp_permille--;
|
||||
|
||||
g_motor_ctrl.fault_state = 0u;
|
||||
}
|
||||
|
||||
bool motor_ctrl_set_speed(int32_t rpm)
|
||||
{
|
||||
g_motor_ctrl.target_speed_rpm = clamp_i32(rpm, -6000, 6000);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool motor_ctrl_set_amp(int32_t amp_permille)
|
||||
{
|
||||
g_motor_ctrl.target_amp_permille = clamp_i32(amp_permille, 0, 1000);
|
||||
return true;
|
||||
}
|
||||
|
||||
void motor_ctrl_set_regular_en(bool en)
|
||||
{
|
||||
g_motor_ctrl.regular_data_en = en;
|
||||
}
|
||||
|
||||
void motor_ctrl_set_regular_period(uint16_t period_ms)
|
||||
{
|
||||
if (period_ms == 0u)
|
||||
period_ms = 1u;
|
||||
g_motor_ctrl.regular_period_ms = period_ms;
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
#include "platform_stub.h"
|
||||
|
||||
static uint32_t s_tick_ms;
|
||||
|
||||
uint32_t platform_tick_ms(void)
|
||||
{
|
||||
return s_tick_ms;
|
||||
}
|
||||
|
||||
void platform_tick_inc_1ms(void)
|
||||
{
|
||||
s_tick_ms++;
|
||||
}
|
||||
|
||||
uint16_t platform_adc_bus_mv(void)
|
||||
{
|
||||
return 12000u;
|
||||
}
|
||||
|
||||
int32_t platform_speed_feedback_rpm(void)
|
||||
{
|
||||
return 1000;
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
#include "stream_srv.h"
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include "motor_ctrl_core.h"
|
||||
#include "platform_stub.h"
|
||||
|
||||
static uint32_t s_last_tx_ms;
|
||||
|
||||
static void pack_u16_le(uint8_t *buf, uint16_t v)
|
||||
{
|
||||
buf[0] = (uint8_t)(v & 0xFFu);
|
||||
buf[1] = (uint8_t)((v >> 8) & 0xFFu);
|
||||
}
|
||||
|
||||
static void pack_i32_le(uint8_t *buf, int32_t v)
|
||||
{
|
||||
uint32_t uv = (uint32_t)v;
|
||||
buf[0] = (uint8_t)(uv & 0xFFu);
|
||||
buf[1] = (uint8_t)((uv >> 8) & 0xFFu);
|
||||
buf[2] = (uint8_t)((uv >> 16) & 0xFFu);
|
||||
buf[3] = (uint8_t)((uv >> 24) & 0xFFu);
|
||||
}
|
||||
|
||||
void stream_srv_process(const CommIf_t *comm)
|
||||
{
|
||||
uint8_t frame[16];
|
||||
uint32_t now;
|
||||
|
||||
if (!g_motor_ctrl.regular_data_en)
|
||||
return;
|
||||
|
||||
now = platform_tick_ms();
|
||||
if ((now - s_last_tx_ms) < g_motor_ctrl.regular_period_ms)
|
||||
return;
|
||||
|
||||
s_last_tx_ms = now;
|
||||
|
||||
frame[0] = 0xA5u;
|
||||
frame[1] = 0x5Au;
|
||||
pack_i32_le(&frame[2], g_motor_ctrl.target_speed_rpm);
|
||||
pack_i32_le(&frame[6], g_motor_ctrl.current_speed_rpm);
|
||||
frame[10] = (uint8_t)g_motor_ctrl.current_amp_permille;
|
||||
frame[11] = g_motor_ctrl.fault_state;
|
||||
pack_u16_le(&frame[12], platform_adc_bus_mv());
|
||||
frame[14] = 0u;
|
||||
frame[15] = 0u;
|
||||
|
||||
(void)comm->tx_bytes(comm->ctx, frame, (uint16_t)sizeof(frame));
|
||||
}
|
||||
@@ -0,0 +1,79 @@
|
||||
#include "transport_i2c.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
volatile uint8_t rx_buf[128];
|
||||
volatile uint16_t rx_len;
|
||||
volatile bool frame_ready;
|
||||
} I2cTransport_t;
|
||||
|
||||
static I2cTransport_t s_i2c;
|
||||
|
||||
static bool i2c_is_ready(void *ctx)
|
||||
{
|
||||
I2cTransport_t *i = (I2cTransport_t *)ctx;
|
||||
return i->frame_ready;
|
||||
}
|
||||
|
||||
static bool i2c_get_frame(void *ctx, CommFrame_t *frame)
|
||||
{
|
||||
I2cTransport_t *i = (I2cTransport_t *)ctx;
|
||||
if (!i->frame_ready)
|
||||
return false;
|
||||
frame->data = (const uint8_t *)i->rx_buf;
|
||||
frame->len = i->rx_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void i2c_clear_frame(void *ctx)
|
||||
{
|
||||
I2cTransport_t *i = (I2cTransport_t *)ctx;
|
||||
i->rx_len = 0u;
|
||||
i->rx_buf[0] = 0u;
|
||||
i->frame_ready = false;
|
||||
}
|
||||
|
||||
static bool i2c_tx_bytes(void *ctx, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
(void)ctx;
|
||||
(void)data;
|
||||
(void)len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static const CommIf_t s_i2c_if = {
|
||||
.ctx = &s_i2c,
|
||||
.is_ready = i2c_is_ready,
|
||||
.get_frame = i2c_get_frame,
|
||||
.clear_frame = i2c_clear_frame,
|
||||
.tx_bytes = i2c_tx_bytes,
|
||||
};
|
||||
|
||||
void transport_i2c_init(void)
|
||||
{
|
||||
(void)memset((void *)&s_i2c, 0, sizeof(s_i2c));
|
||||
}
|
||||
|
||||
const CommIf_t *transport_i2c_get_if(void)
|
||||
{
|
||||
return &s_i2c_if;
|
||||
}
|
||||
|
||||
void transport_i2c_rx_frame_push(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
uint16_t copy_len;
|
||||
|
||||
if ((data == NULL) || (len == 0u) || s_i2c.frame_ready)
|
||||
return;
|
||||
|
||||
copy_len = len;
|
||||
if (copy_len >= (uint16_t)sizeof(s_i2c.rx_buf))
|
||||
copy_len = (uint16_t)(sizeof(s_i2c.rx_buf) - 1u);
|
||||
|
||||
(void)memcpy((void *)s_i2c.rx_buf, data, copy_len);
|
||||
s_i2c.rx_buf[copy_len] = 0u;
|
||||
s_i2c.rx_len = copy_len;
|
||||
s_i2c.frame_ready = true;
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
#include "transport_spi.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
volatile uint8_t rx_buf[128];
|
||||
volatile uint16_t rx_len;
|
||||
volatile bool frame_ready;
|
||||
} SpiTransport_t;
|
||||
|
||||
static SpiTransport_t s_spi;
|
||||
|
||||
static bool spi_is_ready(void *ctx)
|
||||
{
|
||||
SpiTransport_t *s = (SpiTransport_t *)ctx;
|
||||
return s->frame_ready;
|
||||
}
|
||||
|
||||
static bool spi_get_frame(void *ctx, CommFrame_t *frame)
|
||||
{
|
||||
SpiTransport_t *s = (SpiTransport_t *)ctx;
|
||||
if (!s->frame_ready)
|
||||
return false;
|
||||
frame->data = (const uint8_t *)s->rx_buf;
|
||||
frame->len = s->rx_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void spi_clear_frame(void *ctx)
|
||||
{
|
||||
SpiTransport_t *s = (SpiTransport_t *)ctx;
|
||||
s->rx_len = 0u;
|
||||
s->rx_buf[0] = 0u;
|
||||
s->frame_ready = false;
|
||||
}
|
||||
|
||||
static bool spi_tx_bytes(void *ctx, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
(void)ctx;
|
||||
(void)data;
|
||||
(void)len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static const CommIf_t s_spi_if = {
|
||||
.ctx = &s_spi,
|
||||
.is_ready = spi_is_ready,
|
||||
.get_frame = spi_get_frame,
|
||||
.clear_frame = spi_clear_frame,
|
||||
.tx_bytes = spi_tx_bytes,
|
||||
};
|
||||
|
||||
void transport_spi_init(void)
|
||||
{
|
||||
(void)memset((void *)&s_spi, 0, sizeof(s_spi));
|
||||
}
|
||||
|
||||
const CommIf_t *transport_spi_get_if(void)
|
||||
{
|
||||
return &s_spi_if;
|
||||
}
|
||||
|
||||
void transport_spi_rx_frame_push(const uint8_t *data, uint16_t len)
|
||||
{
|
||||
uint16_t copy_len;
|
||||
|
||||
if ((data == NULL) || (len == 0u) || s_spi.frame_ready)
|
||||
return;
|
||||
|
||||
copy_len = len;
|
||||
if (copy_len >= (uint16_t)sizeof(s_spi.rx_buf))
|
||||
copy_len = (uint16_t)(sizeof(s_spi.rx_buf) - 1u);
|
||||
|
||||
(void)memcpy((void *)s_spi.rx_buf, data, copy_len);
|
||||
s_spi.rx_buf[copy_len] = 0u;
|
||||
s_spi.rx_len = copy_len;
|
||||
s_spi.frame_ready = true;
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
#include "transport_uart.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
|
||||
typedef struct
|
||||
{
|
||||
volatile uint8_t rx_buf[128];
|
||||
volatile uint16_t rx_len;
|
||||
volatile bool frame_ready;
|
||||
} UartTransport_t;
|
||||
|
||||
static UartTransport_t s_uart;
|
||||
|
||||
static bool uart_is_ready(void *ctx)
|
||||
{
|
||||
UartTransport_t *u = (UartTransport_t *)ctx;
|
||||
return u->frame_ready;
|
||||
}
|
||||
|
||||
static bool uart_get_frame(void *ctx, CommFrame_t *frame)
|
||||
{
|
||||
UartTransport_t *u = (UartTransport_t *)ctx;
|
||||
if (!u->frame_ready)
|
||||
return false;
|
||||
frame->data = (const uint8_t *)u->rx_buf;
|
||||
frame->len = u->rx_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void uart_clear_frame(void *ctx)
|
||||
{
|
||||
UartTransport_t *u = (UartTransport_t *)ctx;
|
||||
u->rx_len = 0u;
|
||||
u->rx_buf[0] = 0u;
|
||||
u->frame_ready = false;
|
||||
}
|
||||
|
||||
static bool uart_tx_bytes(void *ctx, const uint8_t *data, uint16_t len)
|
||||
{
|
||||
(void)ctx;
|
||||
(void)fwrite(data, 1u, len, stdout);
|
||||
return true;
|
||||
}
|
||||
|
||||
static const CommIf_t s_uart_if = {
|
||||
.ctx = &s_uart,
|
||||
.is_ready = uart_is_ready,
|
||||
.get_frame = uart_get_frame,
|
||||
.clear_frame = uart_clear_frame,
|
||||
.tx_bytes = uart_tx_bytes,
|
||||
};
|
||||
|
||||
void transport_uart_init(void)
|
||||
{
|
||||
(void)memset((void *)&s_uart, 0, sizeof(s_uart));
|
||||
}
|
||||
|
||||
const CommIf_t *transport_uart_get_if(void)
|
||||
{
|
||||
return &s_uart_if;
|
||||
}
|
||||
|
||||
void transport_uart_rx_isr_push_byte(uint8_t ch)
|
||||
{
|
||||
if (s_uart.frame_ready)
|
||||
return;
|
||||
|
||||
if ((ch == '\r') || (ch == '\n'))
|
||||
{
|
||||
if (s_uart.rx_len > 0u)
|
||||
s_uart.frame_ready = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (s_uart.rx_len < (uint16_t)(sizeof(s_uart.rx_buf) - 1u))
|
||||
{
|
||||
s_uart.rx_buf[s_uart.rx_len++] = ch;
|
||||
s_uart.rx_buf[s_uart.rx_len] = 0u;
|
||||
}
|
||||
}
|
||||
@@ -35,7 +35,7 @@ void SYS_Config()
|
||||
sys_delay(3000);
|
||||
|
||||
/* Disable ICE I/O */
|
||||
SYSCFG_SetICEPin2NormalIO(true);
|
||||
//SYSCFG_SetICEPin2NormalIO(true);
|
||||
|
||||
gpio_config();
|
||||
opa_config();
|
||||
|
||||
@@ -57,7 +57,7 @@
|
||||
<listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
|
||||
<listEntry value="4"/>
|
||||
</listAttribute>
|
||||
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <memoryBlockExpressionList context="Context string"/> "/>
|
||||
<stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <memoryBlockExpressionList context="Context string"/> "/>
|
||||
<stringAttribute key="org.eclipse.embedcdt.debug.gdbjtag.core.PERIPHERALS" value="<?xml version="1.0" encoding="UTF-8" standalone="no"?> <peripherals/> "/>
|
||||
<stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
|
||||
</launchConfiguration>
|
||||
|
||||
@@ -0,0 +1,198 @@
|
||||
/**
|
||||
* Copyright (c) 2026 Wisetop. All Rights Reserved.
|
||||
*/
|
||||
/** @file main.c
|
||||
*
|
||||
* @version 0.0.1
|
||||
* @date 2026/04/22
|
||||
* @license
|
||||
* @description
|
||||
*/
|
||||
#include "main.h"
|
||||
|
||||
#include "hal_gpio.h"
|
||||
#include "hal_tim.h"
|
||||
#include "uart_cmd_srv.h"
|
||||
//=============================================================================
|
||||
// Constant Definition
|
||||
//=============================================================================
|
||||
#define EPWM_TARGET_FREQ_HZ (24000U)
|
||||
#define EPWM_TIM_CLOCK_HZ (60000000U)
|
||||
#define PWM_TABLE_SIZE (64U)
|
||||
|
||||
// Example: 50Hz electrical frequency in Q0.32 phase accumulator domain.
|
||||
#define PHASE_TARGET_FREQ_HZ (50U)
|
||||
|
||||
#define PHASE_OFFSET_120_DEG_Q32 (0x55555555UL)
|
||||
#define PHASE_OFFSET_240_DEG_Q32 (0xAAAAAAAAUL)
|
||||
|
||||
//=============================================================================
|
||||
// Macro Definition
|
||||
//=============================================================================
|
||||
|
||||
//=============================================================================
|
||||
// Structure Definition
|
||||
//=============================================================================
|
||||
|
||||
//=============================================================================
|
||||
// Global Data Definition
|
||||
//=============================================================================
|
||||
static const uint16_t pwm_table[PWM_TABLE_SIZE]
|
||||
= { 1250, 1372, 1493, 1612, 1728, 1839, 1944, 2042, 2133, 2216, 2289,
|
||||
2352, 2404, 2446, 2475, 2493, 2499, 2493, 2475, 2446, 2404, 2352,
|
||||
2289, 2216, 2133, 2042, 1944, 1839, 1728, 1612, 1493, 1372, 1250,
|
||||
1127, 1006, 887, 771, 660, 555, 457, 366, 283, 210, 147,
|
||||
95, 53, 24, 6, 0, 6, 24, 53, 95, 147, 210,
|
||||
283, 366, 457, 555, 660, 771, 887, 1006, 1127 };
|
||||
|
||||
static volatile uint32_t phase_acc_base = 0U;
|
||||
static uint32_t phase_acc_step = 0U;
|
||||
|
||||
//=============================================================================
|
||||
// Private Function Definition
|
||||
//=============================================================================
|
||||
static void epwm_set_3phase_duty_from_phase(uint32_t phase_q32)
|
||||
{
|
||||
const uint16_t idx_a = (uint16_t)(phase_q32 >> 26U);
|
||||
const uint16_t idx_b = (uint16_t)((phase_q32 + PHASE_OFFSET_120_DEG_Q32) >> 26U);
|
||||
const uint16_t idx_c = (uint16_t)((phase_q32 + PHASE_OFFSET_240_DEG_Q32) >> 26U);
|
||||
|
||||
EPWM->CCR1 = pwm_table[idx_a & (PWM_TABLE_SIZE - 1U)];
|
||||
EPWM->CCR2 = pwm_table[idx_b & (PWM_TABLE_SIZE - 1U)];
|
||||
EPWM->CCR3 = pwm_table[idx_c & (PWM_TABLE_SIZE - 1U)];
|
||||
}
|
||||
|
||||
__INTERRUPT static void epwm_irq_handler(void)
|
||||
{
|
||||
if ((EPWM->SR & 0x10000U) == 0x10000U)
|
||||
{
|
||||
phase_acc_base += phase_acc_step;
|
||||
epwm_set_3phase_duty_from_phase(phase_acc_base);
|
||||
TIM_ClearITPendingBit(EPWM, 0x10000U); // clear EPWM OVIF
|
||||
}
|
||||
}
|
||||
|
||||
static uint16_t epwm_calc_prescaler(uint32_t tim_clk_hz, uint32_t pwm_freq_hz,
|
||||
uint16_t period)
|
||||
{
|
||||
const uint32_t denom = pwm_freq_hz * ((uint32_t)period + 1U);
|
||||
uint32_t psc = (tim_clk_hz + (denom / 2U)) / denom;
|
||||
|
||||
if (psc == 0U)
|
||||
{
|
||||
psc = 1U;
|
||||
}
|
||||
return (uint16_t)(psc - 1U);
|
||||
}
|
||||
|
||||
static uint16_t epwm_calc_max_period(uint32_t tim_clk_hz, uint32_t pwm_freq_hz)
|
||||
{
|
||||
uint32_t period = (tim_clk_hz / pwm_freq_hz);
|
||||
|
||||
if (period == 0U)
|
||||
{
|
||||
return 0U;
|
||||
}
|
||||
|
||||
period -= 1U;
|
||||
if (period > 0xFFFFU)
|
||||
{
|
||||
period = 0xFFFFU;
|
||||
}
|
||||
return (uint16_t)period;
|
||||
}
|
||||
|
||||
static uint32_t phase_acc_calc_step(uint32_t target_freq_hz,
|
||||
uint32_t update_freq_hz)
|
||||
{
|
||||
return (uint32_t)(((uint64_t)target_freq_hz << 32) / update_freq_hz);
|
||||
}
|
||||
|
||||
static void epwm_init(void)
|
||||
{
|
||||
GPIO_InitTypeDef GPIO_InitStruct = { 0 };
|
||||
TIM_OCInitTypeDef TIM_OCInitStruct = { 0 };
|
||||
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStr = { 0 };
|
||||
sys_irq_attr_t irq_attr = { 0 };
|
||||
const uint16_t period = epwm_calc_max_period(EPWM_TIM_CLOCK_HZ,
|
||||
EPWM_TARGET_FREQ_HZ);
|
||||
const uint16_t prescaler = epwm_calc_prescaler(EPWM_TIM_CLOCK_HZ,
|
||||
EPWM_TARGET_FREQ_HZ, period);
|
||||
|
||||
GPIO_InitStruct.GPIO_Pin = GPIO_Pin_00 | GPIO_Pin_01 | GPIO_Pin_02
|
||||
| GPIO_Pin_03 | GPIO_Pin_04 | GPIO_Pin_05;
|
||||
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
|
||||
GPIO_InitStruct.GPIO_AF_Mode = GPIO_AF_6;
|
||||
GPIO_Init(GPIOA, &GPIO_InitStruct);
|
||||
|
||||
irq_attr.disable_vector = false;
|
||||
irq_attr.trig_mode = SYS_IRQ_TRIGGER_LEVEL;
|
||||
irq_attr.level = SYS_IRQ_LEVEL_H;
|
||||
irq_attr.priority = SYS_IRQ_PRIORITY_MIDDEN;
|
||||
sys_register_IRQ(EPWM_IRQn, epwm_irq_handler, &irq_attr);
|
||||
|
||||
TIM_DeInit(EPWM);
|
||||
|
||||
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStr);
|
||||
TIM_TimeBaseInitStr.TIM_Prescaler = prescaler;
|
||||
TIM_TimeBaseInitStr.TIM_CounterMode = TIM_CounterMode_Up;
|
||||
TIM_TimeBaseInitStr.TIM_Period = period;
|
||||
TIM_TimeBaseInitStr.TIM_ClockDivision = TIM_CKD_Div1;
|
||||
TIM_TimeBaseInitStr.TIM_RepetitionCounter = 0;
|
||||
TIM_TimeBaseInit(EPWM, &TIM_TimeBaseInitStr);
|
||||
TIM_ARRPreloadConfig(EPWM, ENABLE);
|
||||
|
||||
TIM_OCStructInit(&TIM_OCInitStruct);
|
||||
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
|
||||
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
|
||||
TIM_OCInitStruct.TIM_OutputNState = TIM_OutputNState_Enable;
|
||||
TIM_OCInitStruct.TIM_Pulse = pwm_table[0];
|
||||
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
|
||||
TIM_OCInitStruct.TIM_OCNPolarity = TIM_OCNPolarity_High;
|
||||
TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Reset;
|
||||
TIM_OCInitStruct.TIM_OCNIdleState = TIM_OCNIdleState_Reset;
|
||||
|
||||
TIM_OC1Init(EPWM, &TIM_OCInitStruct);
|
||||
TIM_OC2Init(EPWM, &TIM_OCInitStruct);
|
||||
TIM_OC3Init(EPWM, &TIM_OCInitStruct);
|
||||
|
||||
EPWM->CCMR1_OUTPUT_b.OC1PE = 1U;
|
||||
EPWM->CCMR1_OUTPUT_b.OC2PE = 1U;
|
||||
EPWM->CCMR2_OUTPUT_b.OC3PE = 1U;
|
||||
|
||||
EPWM->DIER |= 0x0800U; // overflow interrupt enable
|
||||
TIM_ClearITPendingBit(EPWM, 0x10000U);
|
||||
|
||||
TIM_Cmd(EPWM, ENABLE);
|
||||
TIM_CtrlPWMOutputs(EPWM, ENABLE);
|
||||
epwm_set_3phase_duty_from_phase(phase_acc_base);
|
||||
__enable_irq();
|
||||
}
|
||||
|
||||
static void sysclk_init(void)
|
||||
{
|
||||
SYSCFG_ClkInitTypeDef SysClkInit = { 0 };
|
||||
SysClkInit.ClkSource = SYSCFG_ClkSrc_HSI;
|
||||
SYSCFG_SysClkConfig(&SysClkInit);
|
||||
|
||||
sys_config_systick(SYS_TICK_1_MS);
|
||||
}
|
||||
|
||||
//=============================================================================
|
||||
// Public Function Definition
|
||||
//=============================================================================
|
||||
int main(void)
|
||||
{
|
||||
sysclk_init();
|
||||
phase_acc_step = phase_acc_calc_step(PHASE_TARGET_FREQ_HZ, EPWM_TARGET_FREQ_HZ);
|
||||
epwm_init();
|
||||
|
||||
uart_init();
|
||||
uart_send_msg("%s\r\n", "uart ok");
|
||||
|
||||
while (1)
|
||||
{
|
||||
uart_cmd_process();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,145 @@
|
||||
#include "uart_cmd_srv.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "epwm_drv.h"
|
||||
|
||||
static bool cmd_match_with_arg(const volatile char *buf, const char *prefix, const char **arg_out)
|
||||
{
|
||||
size_t len = strlen(prefix);
|
||||
for (size_t i = 0; i < len; i++)
|
||||
{
|
||||
if (buf[i] != prefix[i])
|
||||
return false;
|
||||
}
|
||||
if (buf[len] == ' ')
|
||||
{
|
||||
*arg_out = (const char *)&buf[len + 1];
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
static int32_t cmd_parse_int_list(const char *arg, int32_t *out, int32_t max_count)
|
||||
{
|
||||
int32_t count = 0;
|
||||
char *end = NULL;
|
||||
const char *p = arg;
|
||||
|
||||
while (*p != '\0' && count < max_count)
|
||||
{
|
||||
while (*p == ' ')
|
||||
p++;
|
||||
|
||||
if (*p == '\0')
|
||||
break;
|
||||
|
||||
out[count] = strtol(p, &end, 10);
|
||||
if (end == p)
|
||||
return -1;
|
||||
|
||||
count++;
|
||||
p = end;
|
||||
}
|
||||
|
||||
while (*p == ' ')
|
||||
p++;
|
||||
|
||||
if (*p != '\0')
|
||||
return -1;
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static bool cmd_parse_int_args(const char *arg, int32_t *out, int32_t expect_count,
|
||||
const char *usage_msg)
|
||||
{
|
||||
int32_t parsed = cmd_parse_int_list(arg, out, expect_count);
|
||||
|
||||
if (parsed != expect_count)
|
||||
{
|
||||
uart_send_msg("%s", usage_msg);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void cmd_handle_val(const char *arg)
|
||||
{
|
||||
int32_t values[8] = { 0 };
|
||||
int32_t parsed = cmd_parse_int_list(arg, values, 8);
|
||||
|
||||
if (parsed <= 0)
|
||||
{
|
||||
uart_send_msg("ERR usage: val n1 n2 ...\r\n");
|
||||
return;
|
||||
}
|
||||
|
||||
for (int32_t i = 0; i < parsed; i++)
|
||||
{
|
||||
uart_send_msg("%d\r\n", values[i]);
|
||||
}
|
||||
}
|
||||
|
||||
void uart_cmd_process(void)
|
||||
{
|
||||
if (!uart_is_frame_ready())
|
||||
return;
|
||||
|
||||
const char *arg = NULL;
|
||||
char *cmd = (char *)g_uart_rx_buf;
|
||||
|
||||
if (strcmp(cmd, "help") == 0)
|
||||
{
|
||||
uart_send_msg("Commands:\r\n");
|
||||
uart_send_msg(" help - show this help\r\n");
|
||||
uart_send_msg(" fw_ver - show firmware version\r\n");
|
||||
uart_send_msg(" pwm_en 1/0 - enable/disable PWM output\r\n");
|
||||
uart_send_msg(" epwm_set_duty d1 d2 d3 - set duty for 3 channels\r\n");
|
||||
uart_send_msg(" val n1 n2 ... - parse integer values\r\n");
|
||||
}
|
||||
else if (strcmp(cmd, "fw_ver") == 0)
|
||||
{
|
||||
uart_send_msg("FW: " FW_VERSION "\r\n");
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd, "pwm_en", &arg))
|
||||
{
|
||||
if (arg[0] == '1')
|
||||
{
|
||||
epwm_en(1);
|
||||
uart_send_msg("OK pwm on\r\n");
|
||||
}
|
||||
else if (arg[0] == '0')
|
||||
{
|
||||
epwm_en(0);
|
||||
uart_send_msg("OK pwm off\r\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
uart_send_msg("ERR usage: pwm_en 1/0\r\n");
|
||||
}
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd, "epwm_set_duty", &arg))
|
||||
{
|
||||
int32_t values[3] = { 0 };
|
||||
if (cmd_parse_int_args(arg, values, 3,
|
||||
"ERR usage: epwm_set_duty duty1 duty2 duty3\r\n"))
|
||||
{
|
||||
epwm_set_duty(values[0], values[1], values[2]);
|
||||
uart_send_msg("OK epwm_set_duty: %d %d %d\r\n", values[0], values[1], values[2]);
|
||||
}
|
||||
}
|
||||
else if (cmd_match_with_arg(cmd, "val", &arg))
|
||||
{
|
||||
cmd_handle_val(arg);
|
||||
}
|
||||
else
|
||||
{
|
||||
uart_send_msg("ERR unknown cmd\r\n");
|
||||
}
|
||||
|
||||
/* Reset for next command */
|
||||
uart_clear_rx_frame();
|
||||
}
|
||||
Reference in New Issue
Block a user