206 lines
6.1 KiB
C
206 lines
6.1 KiB
C
|
|
#include "debug_print.h"
|
|
#include "motor.h"
|
|
#include "common_def.h"
|
|
#include "pwm.h"
|
|
#include "pinctrl.h"
|
|
#include "gpio.h"
|
|
#include "rtc.h"
|
|
#include "timer.h"
|
|
#include "systick.h"
|
|
#include "chip_core_irq.h"
|
|
#include "cmsis_os2.h" // ::CMSIS:RTOS2
|
|
#include "cmsis_compiler.h" // Compiler agnostic definitions
|
|
#include "os_tick.h"
|
|
#include "sys_config.h"
|
|
#include "common_pm.h"
|
|
#include "pm_definition.h"
|
|
|
|
#define ENABLE_PRINT_INFO 1
|
|
#define DEBUG_ENABLE 0
|
|
#if ENABLE_PRINT_INFO
|
|
#define static_print_info(...) sys_motor_log_i(__VA_ARGS__) // 一般信息打印宏控制
|
|
#define static_print_warn(...) sys_motor_log_w(__VA_ARGS__) // 警告信息打印一般常开
|
|
#define static_print_error(...) sys_motor_log_e(__VA_ARGS__) // 错误信息打印一般常开
|
|
#if DEBUG_ENABLE
|
|
#define static_print_debug(...) sys_motor_log_d(__VA_ARGS__)
|
|
#else
|
|
#define static_print_debug(...)
|
|
#endif
|
|
#else
|
|
#define static_print_info(...)
|
|
#define static_print_warn(...)
|
|
#define static_print_error(...)
|
|
#endif
|
|
|
|
#define MOTOR_PWM_GPIO S_AGPIO_R3
|
|
#define MOTOR_PWM_CHANNEL 2
|
|
#define MOTOR_PWM_CYCLE 1000 //1KHZ 周期
|
|
|
|
#define MOTOR_IDLE 0
|
|
#define MOTOR_WORK_ON_CYCLE1 1
|
|
#define MOTOR_WORK_OFF_CYCLE1 2
|
|
#define MOTOR_WORK_STOP 3
|
|
static uint8_t motor_work_state;
|
|
static osTimerId_t motor_timer = NULL;
|
|
static motor_param_t motor_param;
|
|
static uint16_t cycle_cnt;
|
|
static uint8_t start_flag; //1:started 0:closed
|
|
|
|
static void tjd_driver_motor_stop(void);
|
|
|
|
static void tjd_driver_motor_init(void)
|
|
{
|
|
uapi_pwm_close(MOTOR_PWM_CHANNEL);
|
|
uapi_pin_set_mode(MOTOR_PWM_GPIO, HAL_PIO_FUNC_PWMP2_M2);
|
|
uapi_pwm_deinit();
|
|
errcode_t err = uapi_pwm_init();
|
|
if (err != ERRCODE_SUCC) {
|
|
static_print_error("pwm init err code:%d", err);
|
|
}
|
|
start_flag = 0;
|
|
static_print_info("pwm init success");
|
|
}
|
|
|
|
static void motor_lp_event_callback(pm_event_t event)
|
|
{
|
|
if (event == PM_EVENT_SUSPEND) {
|
|
|
|
} else {
|
|
tjd_driver_motor_init();
|
|
}
|
|
}
|
|
|
|
static void tjd_driver_motor_start(uint8_t duty)
|
|
{
|
|
tjd_driver_common_pm_get_ops()->register_lp_on_event(POWER_MODEL_MOTOR_ID, motor_lp_event_callback);
|
|
uapi_pm_add_sleep_veto(PM_ID_MOTOR);
|
|
tjd_driver_motor_init();
|
|
if (duty > 100) {
|
|
static_print_error("duty err:%d", duty);
|
|
return;
|
|
}
|
|
uint32_t clk_freq = uapi_pwm_get_frequency(MOTOR_PWM_CHANNEL);
|
|
uint32_t low_time, high_time;
|
|
uint32_t m = clk_freq / MOTOR_PWM_CYCLE;
|
|
high_time = m*duty/100;
|
|
low_time = m - high_time;
|
|
if (high_time == 0) {
|
|
tjd_driver_motor_stop();
|
|
return;
|
|
}
|
|
|
|
if (start_flag) {
|
|
if (uapi_pwm_update_duty_ratio(MOTOR_PWM_CHANNEL, low_time, high_time) != ERRCODE_SUCC) {
|
|
static_print_error("pwm update duty err");
|
|
return;
|
|
}
|
|
} else {
|
|
pwm_config_t cfg = {
|
|
.low_time = low_time,
|
|
.high_time = high_time,
|
|
.offset_time = 0,
|
|
.cycles = 0,
|
|
.repeat = 1
|
|
};
|
|
if (uapi_pwm_open((uint8_t)MOTOR_PWM_CHANNEL, &cfg) != ERRCODE_SUCC) {
|
|
static_print_error("pwm open err");
|
|
return;
|
|
}
|
|
if (uapi_pwm_start((uint8_t)MOTOR_PWM_CHANNEL) != ERRCODE_SUCC) {
|
|
static_print_error("pwm start err");
|
|
return;
|
|
}
|
|
start_flag = 1;
|
|
}
|
|
static_print_debug("motor start success");
|
|
}
|
|
|
|
static void tjd_driver_motor_stop(void)
|
|
{
|
|
tjd_driver_common_pm_get_ops()->unregister_lp_on_event(POWER_MODEL_MOTOR_ID);
|
|
if (uapi_pwm_stop(MOTOR_PWM_CHANNEL) != ERRCODE_SUCC) {
|
|
static_print_error("pwm stop err");
|
|
start_flag = 0;
|
|
uapi_pm_remove_sleep_veto(PM_ID_MOTOR);
|
|
return;
|
|
}
|
|
uapi_pm_remove_sleep_veto(PM_ID_MOTOR);
|
|
start_flag = 0;
|
|
static_print_debug("motor stop success");
|
|
}
|
|
|
|
static void motor_timer_callback(void *argument)
|
|
{
|
|
uint16_t ms;
|
|
if (motor_work_state == MOTOR_WORK_ON_CYCLE1) {
|
|
motor_work_state = MOTOR_WORK_OFF_CYCLE1;
|
|
tjd_driver_motor_stop();
|
|
if (cycle_cnt >= 2) {
|
|
ms = motor_param.cycle1_off;
|
|
} else if (cycle_cnt == 1 && motor_param.cycle1_2_cnt != 0) {
|
|
ms = motor_param.cycle1_2_period + motor_param.cycle1_off;
|
|
} else {
|
|
ms = 0;
|
|
}
|
|
if (ms == 0) {
|
|
osTimerStop(motor_timer);
|
|
} else {
|
|
osTimerStart(motor_timer, ms);
|
|
}
|
|
} else if (motor_work_state == MOTOR_WORK_OFF_CYCLE1) {
|
|
motor_work_state = MOTOR_WORK_ON_CYCLE1;
|
|
if (cycle_cnt > 0)
|
|
cycle_cnt--;
|
|
if (cycle_cnt == 0) {
|
|
if (motor_param.cycle1_2_cnt > 0) {
|
|
motor_param.cycle1_2_cnt--;
|
|
cycle_cnt = motor_param.cycle1_cnt;
|
|
}
|
|
if (motor_param.cycle1_2_cnt == 0) {
|
|
motor_work_state = MOTOR_IDLE;
|
|
osTimerStop(motor_timer);
|
|
tjd_driver_motor_stop();
|
|
} else {
|
|
tjd_driver_motor_start(motor_param.duty);
|
|
osTimerStart(motor_timer, motor_param.cycle1_on);
|
|
}
|
|
|
|
} else {
|
|
tjd_driver_motor_start(motor_param.duty);
|
|
osTimerStart(motor_timer, motor_param.cycle1_on);
|
|
}
|
|
}
|
|
}
|
|
|
|
void tjd_driver_motor_user_start(motor_param_t *param)
|
|
{
|
|
memcpy((uint8_t *)&motor_param, param, sizeof(motor_param_t));
|
|
motor_work_state = MOTOR_WORK_ON_CYCLE1;
|
|
if (motor_param.cycle1_cnt == 0) {
|
|
motor_param.cycle1_cnt = 1;
|
|
}
|
|
if (motor_param.cycle1_2_cnt == 0) {
|
|
motor_param.cycle1_2_cnt = 1;
|
|
}
|
|
cycle_cnt = motor_param.cycle1_cnt;
|
|
tjd_driver_motor_start(motor_param.duty);
|
|
if(motor_timer == NULL) {
|
|
motor_timer = osTimerNew((osTimerFunc_t)motor_timer_callback, osTimerOnce, NULL, NULL);
|
|
osTimerStart(motor_timer, motor_param.cycle1_on);
|
|
} else {
|
|
if (osTimerIsRunning(motor_timer))
|
|
osTimerStop(motor_timer);
|
|
osTimerStart(motor_timer, motor_param.cycle1_on);
|
|
}
|
|
}
|
|
|
|
void tjd_driver_motor_user_stop(void)
|
|
{
|
|
if (osTimerIsRunning(motor_timer))
|
|
osTimerStop(motor_timer);
|
|
tjd_driver_motor_stop();
|
|
}
|
|
|
|
|