Skip to content

C
car-controlserver
Commit 6edd4759 authored by 957dd's avatar 957dd
Browse files
Options

加入了app配网，0107进行了调整，视频流还需调整

parent 7f0fd331
Hide whitespace changes
Inline Side-by-side
Showing with 759 additions and 205 deletions
build/main
View file @ 6edd4759
No preview for this file type
drivers/devicecontrol/car/car0107_control.c
View file @ 6edd4759
......@@ -14,16 +14,19 @@
#define CAR0107_ESC_US_NEUTRAL 1500
#define CAR0107_ESC_DEADZONE_LO 1450
#define CAR0107_ESC_DEADZONE_HI 1550
#define CAR0107_ESC_RAMP_STEP_NEUTRAL 8
#define CAR0107_ESC_RAMP_STEP_RUN 4 /* 正常前进/后退线性爬升：约 4us/tick */
#define CAR0107_ESC_NEUTRAL_HOLD_TICKS 8
#define CAR0107_ESC_BRAKE_HOLD_TICKS 20 /* 约 500ms @25ms tick，满刹保持 */
#define CAR0107_ESC_RAMP_STEP_RUN 4 /* 前进线性爬升：约 4us/tick */
#define CAR0107_ESC_RAMP_STEP_BACK 2 /* 后退更慢，不得快于前进 */
#define CAR0107_ESC_BRAKE_HOLD_MS 100 /* 1200us 刹车目标保持50ms */
#define CAR0107_ESC_BRAKE_MIN_TICKS 2
#define CAR0107_ESC_REVERSE_BRAKE_MS 50 /* 前进切后退：短刹车目标50ms */
#define CAR0107_ESC_REVERSE_BRAKE_TICKS 2
#define CAR0107_ESC_NEUTRAL_UNLOCK_MS 40 /* 刹车后保持两个50Hz中位周期解锁 */
#define CAR0107_FWD_BRAKE_US 1100 /* RC无刷电调前进刹车信号 */
#define CAR0107_PHONE_VAL_MAX 200 /* App 前后油门最大值 */
#define CAR0107_FORWARD_PWM_CAP 83 /* 前进 PWM 封顶 */
#define CAR0107_FWD_US_RUN_MIN (79 * 20) /* val=53 前进最低行走 ~1580us */
#define CAR0107_FWD_US_RUN_MAX (CAR0107_FORWARD_PWM_CAP * 20) /* val=200 ~1660us */
#define CAR0107_BACK_US_RUN_MIN (71 * 20) /* val=53 后退最低行走 ~1420us */
#define CAR0107_BACK_US_RUN_MAX (58 * 20) /* val=200 后退最深 ~1160us */
#define CAR0107_FWD_US_RUN_MIN 1560 /* val=53 前进最低行走 ~1580us */
#define CAR0107_FWD_US_RUN_MAX 1700 /* 前进最大速度再降20%：val=200 -> 1700us */
#define CAR0107_BACK_US_RUN_MIN 1440 /* val=53 起步即动（须低于死区 1450） */
#define CAR0107_BACK_US_RUN_MAX 1300 /* 后退最大速度再降20%：val=200 -> 1300us */
typedef enum {
CAR0107_ESC_DIR_FORWARD = 1, /* App mode1 前进：高脉宽（与 0101 相反） */
......@@ -31,16 +34,18 @@ typedef enum {
} car0107_esc_dir_t;
typedef enum {
CAR0107_ESC_STOP = 0,
CAR0107_ESC_NEUTRAL = 0,
CAR0107_ESC_FORWARD,
CAR0107_ESC_REVERSE,
CAR0107_ESC_BRAKE_THEN_NEUTRAL,
CAR0107_ESC_BRAKE_THEN_REVERSE,
CAR0107_ESC_NEUTRAL_UNLOCK,
CAR0107_ESC_TRANSITION
} car0107_esc_state_t;
typedef enum {
CAR0107_TRANS_BRAKE = 0, /* 前进松手/换后退：1000us 满刹 */
CAR0107_TRANS_NEUTRAL_HOLD, /* 回中位 1500us */
CAR0107_TRANS_RAMP_RUN /* 慢速爬到目标油门 */
CAR0107_TRANS_BRAKE = 0, /* mode1 val<52：1200us 约50ms后回中 */
CAR0107_TRANS_RAMP_RUN /* 回中后正常线性爬升 */
} car0107_trans_phase_t;
typedef struct {
......@@ -52,8 +57,13 @@ typedef struct {
int pending_dir;
int pending_gval;
int brake_dir;
int brake_ticks;
int neutral_hold_ticks;
long long brake_until_ms;
int brake_hold_ticks;
long long neutral_unlock_until_ms;
int last_drive_dir;
int last_nonzero_mode;
long long pending_stop_start_ms;
long long last_input_ms;
} car0107_esc_ctx_t;
#define STEER_STEP_TURN 60
......@@ -89,7 +99,7 @@ static int s_phone_mode2_val = 0;
static int s_phone_last_mode = 0; /* 最近更新的通道，双通道同时有油时以它为准 */
static car0107_esc_ctx_t s_esc = {
.state = CAR0107_ESC_STOP,
.state = CAR0107_ESC_NEUTRAL,
.trans_phase = CAR0107_TRANS_BRAKE,
.current_us = CAR0107_ESC_US_NEUTRAL,
.target_us = CAR0107_ESC_US_NEUTRAL,
......@@ -97,10 +107,26 @@ static car0107_esc_ctx_t s_esc = {
.pending_dir = 0,
.pending_gval = 0,
.brake_dir = 0,
.brake_ticks = 0,
.neutral_hold_ticks = 0,
.brake_until_ms = 0,
.brake_hold_ticks = 0,
.neutral_unlock_until_ms = 0,
.last_drive_dir = 0,
.last_nonzero_mode = 0,
.pending_stop_start_ms = 0,
.last_input_ms = 0,
};
/* 专用电调线程：本文件 pthread_create，非全局线程池；仅 0107 设备使用 */
#define CAR0107_ESC_THREAD_MS 20
#define CAR0107_ESC_BRAKE_THREAD_MS 10
static pthread_t s_esc_thread;
static int s_esc_thread_started = 0;
static volatile int s_esc_thread_run = 0;
static pthread_mutex_t s_esc_start_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_mutex_t s_esc_wake_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t s_esc_wake_cond = PTHREAD_COND_INITIALIZER;
static int car0107_clamp(int val, int lo, int hi)
{
if (val < lo) return lo;
......@@ -213,13 +239,37 @@ static int car0107_phone_clamp_val(int raw_val)
return car0107_clamp(raw_val, CAR0107_ESC_GVAL_MIN, CAR0107_PHONE_VAL_MAX);
}
static int car0107_phone_is_active(int raw_val)
{
return raw_val > CAR0107_ESC_RELEASE_MAX;
}
static int car0107_mode1_is_release(int raw_val)
{
return raw_val <= CAR0107_ESC_RELEASE_MAX;
}
static int car0107_esc_in_forward_context(void)
{
if (s_esc.state == CAR0107_ESC_FORWARD) {
return 1;
}
if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) {
return 1;
}
if (s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
return 1;
}
return 0;
}
/*
* 仅前进松手/换后退时打 1000us 刹车。
* 正常后退最深约 1160us，与刹车 1000us 分开。
* 前进松手 mode1 val<52：1200us 目标保持50ms，再回中。
* 前进中有后退意图：同样先刹车，再按 mode2 慢爬。
*/
static int car0107_forward_brake_us(void)
{
return CAR0107_ESC_US_MIN;
return CAR0107_FWD_BRAKE_US;
}
static int car0107_forward_brake_pwm(void)
......@@ -227,6 +277,19 @@ static int car0107_forward_brake_pwm(void)
return car0107_forward_brake_us() / 20;
}
static void car0107_esc_begin_neutral_stop(void);
static void car0107_esc_clear_pending_stop(void)
{
s_esc.brake_dir = 0;
s_esc.brake_until_ms = 0;
s_esc.brake_hold_ticks = 0;
s_esc.neutral_unlock_until_ms = 0;
s_esc.pending_stop_start_ms = 0;
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
}
static void car0107_esc_write_pwm(int pwm_val)
{
pwm_val = car0107_clamp(pwm_val, CAR0107_ESC_US_MIN / 20, CAR0107_ESC_US_MAX / 20);
......@@ -234,40 +297,122 @@ static void car0107_esc_write_pwm(int pwm_val)
s_esc.current_us = pwm_val * 20;
}
static void car0107_esc_begin_forward_brake(int pending_dir, int pending_gval)
static void car0107_esc_begin_forward_brake(void)
{
s_esc.state = CAR0107_ESC_TRANSITION;
long long now = get_current_time_millis();
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL) {
car0107_esc_write_pwm(car0107_forward_brake_pwm());
return;
}
car0107_esc_clear_pending_stop();
s_esc.state = CAR0107_ESC_BRAKE_THEN_NEUTRAL;
s_esc.trans_phase = CAR0107_TRANS_BRAKE;
s_esc.pending_dir = pending_dir;
s_esc.pending_gval = pending_gval;
s_esc.pending_stop_start_ms = now;
s_esc.brake_dir = CAR0107_ESC_DIR_BACK;
s_esc.brake_ticks = CAR0107_ESC_BRAKE_HOLD_TICKS;
s_esc.neutral_hold_ticks = 0;
s_esc.brake_until_ms = now + CAR0107_ESC_BRAKE_HOLD_MS;
s_esc.brake_hold_ticks = CAR0107_ESC_BRAKE_MIN_TICKS;
s_esc.target_us = car0107_forward_brake_us();
s_esc.current_us = s_esc.target_us;
car0107_esc_write_pwm(car0107_forward_brake_pwm());
}
static void car0107_esc_begin_brake_then_reverse(int reverse_val,
int reuse_current_brake)
{
long long now = get_current_time_millis();
long long brake_start_ms = now;
int elapsed_brake_ticks = 0;
int remaining_brake_ticks = CAR0107_ESC_REVERSE_BRAKE_TICKS;
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
s_esc.pending_gval = reverse_val;
return; /* 连续后退帧只更新目标，不重置短刹车计时 */
}
if (reuse_current_brake && s_esc.pending_stop_start_ms > 0) {
brake_start_ms = s_esc.pending_stop_start_ms;
elapsed_brake_ticks = CAR0107_ESC_BRAKE_MIN_TICKS -
s_esc.brake_hold_ticks;
if (elapsed_brake_ticks < 0) {
elapsed_brake_ticks = 0;
}
remaining_brake_ticks = CAR0107_ESC_REVERSE_BRAKE_TICKS -
elapsed_brake_ticks;
if (remaining_brake_ticks < 0) {
remaining_brake_ticks = 0;
}
}
car0107_esc_clear_pending_stop();
s_esc.state = CAR0107_ESC_BRAKE_THEN_REVERSE;
s_esc.trans_phase = CAR0107_TRANS_BRAKE;
s_esc.pending_dir = CAR0107_ESC_DIR_BACK;
s_esc.pending_gval = reverse_val;
s_esc.brake_dir = CAR0107_ESC_DIR_BACK;
s_esc.brake_until_ms = brake_start_ms + CAR0107_ESC_REVERSE_BRAKE_MS;
s_esc.brake_hold_ticks = remaining_brake_ticks;
s_esc.target_us = car0107_forward_brake_us();
car0107_esc_write_pwm(car0107_forward_brake_pwm());
if (reuse_current_brake) {
my_zlog_info("car0107 ESC reuse current brake: elapsed=%lldms ticks=%d remaining_ticks=%d",
now - brake_start_ms, elapsed_brake_ticks,
remaining_brake_ticks);
}
}
static void car0107_esc_cancel_forward_brake(int fwd_val)
{
car0107_esc_clear_pending_stop();
if (fwd_val >= CAR0107_ESC_GVAL_MIN) {
s_esc.cmd_dir = CAR0107_ESC_DIR_FORWARD;
s_esc.target_us = car0107_throttle_to_us(CAR0107_ESC_DIR_FORWARD, fwd_val);
s_esc.state = CAR0107_ESC_TRANSITION;
s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
return;
}
car0107_esc_begin_neutral_stop();
}
static void car0107_esc_begin_neutral_stop(void)
{
s_esc.cmd_dir = 0;
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
s_esc.brake_dir = 0;
s_esc.brake_ticks = 0;
s_esc.neutral_hold_ticks = 0;
car0107_esc_clear_pending_stop();
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
s_esc.state = CAR0107_ESC_NEUTRAL;
}
static void car0107_esc_begin_neutral_unlock(void)
{
car0107_esc_clear_pending_stop();
s_esc.cmd_dir = 0;
s_esc.state = CAR0107_ESC_NEUTRAL_UNLOCK;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
s_esc.state = CAR0107_ESC_STOP;
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
s_esc.neutral_unlock_until_ms = get_current_time_millis() +
CAR0107_ESC_NEUTRAL_UNLOCK_MS;
car0107_esc_write_pwm(CAR0107_ESC_US_NEUTRAL / 20);
my_zlog_info("car0107 ESC BRAKE_THEN_NEUTRAL -> NEUTRAL_UNLOCK (%dms, keep 1500us)",
CAR0107_ESC_NEUTRAL_UNLOCK_MS);
}
static int car0107_esc_forward_motion_active(void)
{
if (s_esc.cmd_dir != CAR0107_ESC_DIR_FORWARD) {
return 0;
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
return 1;
}
if (s_esc.state == CAR0107_ESC_FORWARD) {
return 1;
}
if (s_esc.cmd_dir != CAR0107_ESC_DIR_FORWARD) {
return 0;
}
if (s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
return 1;
......@@ -280,6 +425,17 @@ static int car0107_esc_forward_motion_active(void)
return 0;
}
static int car0107_esc_should_forward_brake(void)
{
if (car0107_esc_forward_motion_active()) {
return 1;
}
if (s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
return 1;
}
return 0;
}
static int car0107_esc_back_motion_active(void)
{
if (s_esc.cmd_dir != CAR0107_ESC_DIR_BACK) {
......@@ -322,9 +478,11 @@ int car0107_esc_is_active(void)
int active;
pthread_mutex_lock(&s_car0107_hw_mutex);
if (s_esc.state == CAR0107_ESC_TRANSITION) {
active = (s_esc.trans_phase == CAR0107_TRANS_BRAKE);
} else if (s_esc.state == CAR0107_ESC_STOP) {
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE ||
s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
active = 1;
} else if (s_esc.state == CAR0107_ESC_NEUTRAL) {
active = 0;
} else {
active = (s_esc.current_us < CAR0107_ESC_DEADZONE_LO ||
......@@ -355,55 +513,80 @@ static void car0107_esc_write_us(int us)
static void car0107_esc_start_ramp(int dir, int throttle_val)
{
int from_stop = (s_esc.state == CAR0107_ESC_STOP);
int from_stop = (s_esc.state == CAR0107_ESC_NEUTRAL);
s_esc.cmd_dir = dir;
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
s_esc.brake_dir = 0;
car0107_esc_clear_pending_stop();
s_esc.last_drive_dir = dir;
s_esc.last_nonzero_mode = dir;
s_esc.target_us = car0107_throttle_to_us(dir, throttle_val);
s_esc.state = CAR0107_ESC_TRANSITION;
s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
if (from_stop) {
/* val>52 首帧立即到对应脉宽（53=最低行走速度） */
if (from_stop || dir == CAR0107_ESC_DIR_BACK) {
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
}
/* 从中位起步：val>=53 立刻跳到映射脉宽，后续只跟目标线性 */
if (from_stop && (dir == CAR0107_ESC_DIR_FORWARD || dir == CAR0107_ESC_DIR_BACK)) {
s_esc.current_us = s_esc.target_us;
}
}
static void car0107_esc_begin_transition(int pending_dir, int pending_gval)
static void car0107_esc_phone_direct_pwm(int dir, int throttle_val)
{
if (s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.pending_dir == pending_dir &&
s_esc.trans_phase != CAR0107_TRANS_BRAKE) {
s_esc.pending_gval = pending_gval;
return;
car0107_esc_state_t old_state = s_esc.state;
int old_dir = s_esc.last_drive_dir;
if (old_state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
old_state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
my_zlog_info("car0107 ESC cancel old brake/neutral timer");
}
car0107_esc_clear_pending_stop();
s_esc.cmd_dir = dir;
s_esc.last_drive_dir = dir;
s_esc.last_nonzero_mode = dir;
s_esc.target_us = car0107_throttle_to_us(dir, throttle_val);
s_esc.current_us = s_esc.target_us;
s_esc.state = (dir == CAR0107_ESC_DIR_FORWARD) ?
CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
car0107_esc_output_us(s_esc.current_us, 0);
/* 前进中换后退：先 1000us 刹车 */
car0107_esc_begin_forward_brake(pending_dir, pending_gval);
if (dir == CAR0107_ESC_DIR_FORWARD) {
if (old_dir == CAR0107_ESC_DIR_BACK && old_state != CAR0107_ESC_NEUTRAL) {
my_zlog_info("car0107 ESC REVERSE -> FORWARD DIRECT PWM (%dus)",
s_esc.current_us);
} else if (old_state == CAR0107_ESC_NEUTRAL) {
my_zlog_info("car0107 ESC NEUTRAL -> FORWARD DIRECT PWM (%dus)",
s_esc.current_us);
}
} else if (old_state == CAR0107_ESC_NEUTRAL) {
my_zlog_info("car0107 ESC NEUTRAL -> REVERSE_DIRECT_PWM (%dus)",
s_esc.current_us);
}
}
static void car0107_esc_request_stop(void)
static int car0107_esc_ramp_step_for_dir(int dir)
{
if (car0107_esc_forward_motion_active()) {
car0107_esc_begin_forward_brake(0, 0);
return;
if (dir == CAR0107_ESC_DIR_BACK) {
return CAR0107_ESC_RAMP_STEP_BACK;
}
return CAR0107_ESC_RAMP_STEP_RUN;
}
if (car0107_esc_back_motion_active()) {
car0107_esc_begin_neutral_stop();
static void car0107_esc_request_stop(void)
{
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
return;
}
if (s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.trans_phase == CAR0107_TRANS_BRAKE) {
if (car0107_esc_should_forward_brake()) {
car0107_esc_begin_forward_brake();
return;
}
if (s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
if (car0107_esc_back_motion_active()) {
car0107_esc_begin_neutral_stop();
return;
}
......@@ -417,19 +600,40 @@ static void car0107_esc_request(int dir, int gval)
return;
}
if (s_esc.state == CAR0107_ESC_STOP) {
if (s_esc.state == CAR0107_ESC_NEUTRAL) {
car0107_esc_start_ramp(dir, gval);
return;
}
if (s_esc.state == CAR0107_ESC_TRANSITION) {
if (s_esc.trans_phase == CAR0107_TRANS_BRAKE ||
s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
s_esc.pending_dir = dir;
s_esc.pending_gval = gval;
return;
if (s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
s_esc.pending_dir = dir;
s_esc.pending_gval = gval;
return;
}
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
if (dir == CAR0107_ESC_DIR_FORWARD) {
car0107_esc_cancel_forward_brake(gval);
s_esc.last_drive_dir = dir;
} else {
car0107_esc_start_ramp(dir, gval);
}
return;
}
if (s_esc.state == CAR0107_ESC_TRANSITION) {
if (s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD &&
dir == CAR0107_ESC_DIR_BACK) {
car0107_esc_start_ramp(dir, gval);
return;
}
if (s_esc.cmd_dir == CAR0107_ESC_DIR_BACK &&
dir == CAR0107_ESC_DIR_FORWARD) {
car0107_esc_start_ramp(dir, gval);
return;
}
s_esc.target_us = car0107_throttle_to_us(s_esc.cmd_dir, gval);
return;
}
......@@ -442,9 +646,9 @@ static void car0107_esc_request(int dir, int gval)
return;
}
/* 前进切后退：先 1000us 刹车 */
/* 前进切后退：不刹车，从当前脉宽慢速后退 */
if (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD && dir == CAR0107_ESC_DIR_BACK) {
car0107_esc_begin_transition(dir, gval);
car0107_esc_start_ramp(dir, gval);
return;
}
......@@ -455,51 +659,113 @@ static void car0107_esc_request(int dir, int gval)
s_esc.state = (dir == CAR0107_ESC_DIR_FORWARD) ? CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
}
static int car0107_esc_ramp_step_for(int current, int target)
static int car0107_esc_ramp_step_for(int current, int target, int dir)
{
(void)current;
(void)target;
return CAR0107_ESC_RAMP_STEP_RUN;
return car0107_esc_ramp_step_for_dir(dir);
}
static void car0107_esc_apply_phone(int dir, int gval)
static void car0107_phone_apply_merged(void)
{
if (gval < CAR0107_ESC_GVAL_MIN) {
int mode1_raw = s_phone_mode1_val;
int mode2_raw = s_phone_mode2_val;
int fwd_active = car0107_phone_is_active(mode1_raw);
int back_active = car0107_phone_is_active(mode2_raw);
int fwd_val = car0107_phone_clamp_val(mode1_raw);
int back_val = car0107_phone_clamp_val(mode2_raw);
int wanted_dir = 0;
int wanted_val = 0;
if (fwd_active && back_active) {
wanted_dir = (s_phone_last_mode == 2) ?
CAR0107_ESC_DIR_BACK : CAR0107_ESC_DIR_FORWARD;
wanted_val = (wanted_dir == CAR0107_ESC_DIR_BACK) ? back_val : fwd_val;
} else if (fwd_active && s_phone_last_mode == 1) {
wanted_dir = CAR0107_ESC_DIR_FORWARD;
wanted_val = fwd_val;
} else if (back_active && s_phone_last_mode == 2) {
wanted_dir = CAR0107_ESC_DIR_BACK;
wanted_val = back_val;
}
if (wanted_dir == 0) {
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
return;
}
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
my_zlog_info("car0107 ESC cancel old brake/neutral timer (reverse released)");
car0107_esc_begin_neutral_stop();
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
return;
}
if (s_esc.state == CAR0107_ESC_NEUTRAL) {
return;
}
if (s_esc.state == CAR0107_ESC_FORWARD ||
(s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN &&
s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ||
s_esc.current_us > CAR0107_ESC_DEADZONE_HI) {
my_zlog_info("car0107 ESC FORWARD_LOW_VAL_DO_NOT_DRIVE_FORWARD: BRAKE %dus NOW",
car0107_forward_brake_us());
car0107_esc_begin_forward_brake();
} else {
/* 后退或非前进状态松手：不进入换向判断，直接回中。 */
if (s_esc.state == CAR0107_ESC_REVERSE) {
my_zlog_info("car0107 ESC REVERSE -> NEUTRAL_DIRECT");
}
car0107_esc_begin_neutral_stop();
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
}
return;
}
car0107_esc_request(dir, gval);
/* 不在此处瞬间写满 PWM；刹车/回中/慢爬均由 esc_tick 输出 */
}
s_esc.last_input_ms = get_current_time_millis();
s_esc.last_nonzero_mode = wanted_dir;
static void car0107_phone_apply_merged(void)
{
int fwd = s_phone_mode1_val;
int back = s_phone_mode2_val;
int fwd_val = car0107_phone_clamp_val(fwd);
int back_val = car0107_phone_clamp_val(back);
if (s_esc.state == CAR0107_ESC_NEUTRAL_UNLOCK) {
s_esc.pending_dir = wanted_dir;
s_esc.pending_gval = wanted_val;
my_zlog_info("car0107 ESC NEUTRAL_UNLOCK queue %s until minimum neutral hold completes",
wanted_dir == CAR0107_ESC_DIR_FORWARD ? "FORWARD" : "REVERSE");
return;
}
/* mode1/mode2 收到 val=0 或 <=52 都表示停下；以最近更新的通道为准 */
if (s_phone_last_mode == 2) {
if (back_val > 0) {
car0107_esc_apply_phone(CAR0107_ESC_DIR_BACK, back_val);
} else {
car0107_esc_request_stop();
}
if (wanted_dir == CAR0107_ESC_DIR_FORWARD) {
/* 后退/停止/刹车中切前进，都取消旧定时器并直接前进。 */
car0107_esc_phone_direct_pwm(wanted_dir, wanted_val);
return;
}
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
s_esc.pending_gval = wanted_val;
return;
}
if (s_phone_last_mode == 1) {
if (fwd_val > 0) {
car0107_esc_apply_phone(CAR0107_ESC_DIR_FORWARD, fwd_val);
if (s_esc.state == CAR0107_ESC_FORWARD ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
(s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD)) {
int reuse_current_brake =
(s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL);
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL) {
my_zlog_info("car0107 ESC BRAKE_THEN_NEUTRAL -> BRAKE_THEN_REVERSE (reuse brake)");
} else {
car0107_esc_request_stop();
my_zlog_info("car0107 ESC FORWARD -> BRAKE_THEN_REVERSE");
}
my_zlog_info("car0107 ESC cancel old brake/neutral timer");
car0107_esc_begin_brake_then_reverse(wanted_val,
reuse_current_brake);
return;
}
if (fwd_val == 0 && back_val == 0) {
car0107_esc_request_stop();
}
/* NEUTRAL->REVERSE、REVERSE继续后退：直接改 PWM，不刹车。 */
car0107_esc_phone_direct_pwm(CAR0107_ESC_DIR_BACK, wanted_val);
}
static int car0107_esc_ramp_toward(int current, int target, int step)
......@@ -521,10 +787,10 @@ static int car0107_esc_ramp_toward(int current, int target, int step)
static void car0107_esc_tick(void)
{
switch (s_esc.state) {
case CAR0107_ESC_STOP:
s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
CAR0107_ESC_US_NEUTRAL,
CAR0107_ESC_RAMP_STEP_NEUTRAL);
case CAR0107_ESC_NEUTRAL:
/* NEUTRAL 是持续输出状态，不是停止 PWM。 */
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
break;
case CAR0107_ESC_FORWARD:
......@@ -532,61 +798,103 @@ static void car0107_esc_tick(void)
s_esc.current_us = car0107_esc_ramp_toward(
s_esc.current_us,
s_esc.target_us,
car0107_esc_ramp_step_for(s_esc.current_us, s_esc.target_us));
car0107_esc_ramp_step_for(s_esc.current_us, s_esc.target_us,
s_esc.cmd_dir));
break;
case CAR0107_ESC_TRANSITION:
if (s_esc.trans_phase == CAR0107_TRANS_BRAKE) {
case CAR0107_ESC_BRAKE_THEN_NEUTRAL:
car0107_esc_write_pwm(car0107_forward_brake_pwm());
if (s_esc.brake_hold_ticks > 0) {
s_esc.brake_hold_ticks--;
}
if (s_esc.brake_hold_ticks <= 0 &&
get_current_time_millis() >= s_esc.brake_until_ms) {
car0107_esc_begin_neutral_unlock();
}
break;
case CAR0107_ESC_BRAKE_THEN_REVERSE:
if (s_esc.brake_dir == CAR0107_ESC_DIR_BACK) {
car0107_esc_write_pwm(car0107_forward_brake_pwm());
if (s_esc.brake_ticks > 0) {
s_esc.brake_ticks--;
if (s_esc.brake_hold_ticks > 0) {
s_esc.brake_hold_ticks--;
}
if (s_esc.brake_ticks <= 0) {
s_esc.trans_phase = CAR0107_TRANS_NEUTRAL_HOLD;
s_esc.neutral_hold_ticks = CAR0107_ESC_NEUTRAL_HOLD_TICKS;
if (s_esc.brake_hold_ticks <= 0 &&
get_current_time_millis() >= s_esc.brake_until_ms) {
s_esc.brake_dir = 0;
s_esc.brake_until_ms = get_current_time_millis() +
CAR0107_ESC_NEUTRAL_UNLOCK_MS;
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
my_zlog_info("car0107 ESC BRAKE_THEN_REVERSE -> NEUTRAL_UNLOCK (%dms)",
CAR0107_ESC_NEUTRAL_UNLOCK_MS);
}
} else if (s_esc.trans_phase == CAR0107_TRANS_NEUTRAL_HOLD) {
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
if (s_esc.neutral_hold_ticks > 0) {
s_esc.neutral_hold_ticks--;
} else {
int reverse_val = s_esc.pending_gval;
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
if (get_current_time_millis() >= s_esc.brake_until_ms) {
car0107_esc_clear_pending_stop();
s_esc.cmd_dir = CAR0107_ESC_DIR_BACK;
s_esc.last_drive_dir = CAR0107_ESC_DIR_BACK;
s_esc.last_nonzero_mode = CAR0107_ESC_DIR_BACK;
s_esc.target_us = car0107_throttle_to_us(CAR0107_ESC_DIR_BACK,
reverse_val);
/* 起步点直接跳到刚过死区(1440us)，让 val>=53 立即越过死区开始后退；
* 再按 2us/tick 慢爬到 target，避免大 val 突跳到全速冲击电调/齿轮 */
s_esc.current_us = (s_esc.target_us >= CAR0107_BACK_US_RUN_MIN) ?
CAR0107_BACK_US_RUN_MIN : s_esc.target_us;
s_esc.state = CAR0107_ESC_TRANSITION;
s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
car0107_esc_output_us(s_esc.current_us, 0);
my_zlog_info("car0107 ESC BRAKE_THEN_REVERSE -> RAMP_REVERSE (target %dus, start %dus past deadzone)",
s_esc.target_us, s_esc.current_us);
}
if (s_esc.neutral_hold_ticks <= 0) {
if (s_esc.pending_dir == 0) {
s_esc.state = CAR0107_ESC_STOP;
s_esc.cmd_dir = 0;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
s_esc.trans_phase = CAR0107_TRANS_BRAKE;
}
break;
case CAR0107_ESC_NEUTRAL_UNLOCK:
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
if (get_current_time_millis() >= s_esc.neutral_unlock_until_ms) {
int pending_dir = s_esc.pending_dir;
int pending_gval = s_esc.pending_gval;
car0107_esc_clear_pending_stop();
s_esc.state = CAR0107_ESC_NEUTRAL;
s_esc.cmd_dir = 0;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
my_zlog_info("car0107 ESC NEUTRAL_UNLOCK -> NEUTRAL (keep 1500us continuously)");
if (pending_dir != 0 && pending_gval >= CAR0107_ESC_GVAL_MIN) {
if (s_control_source == CTRL_PHONE) {
car0107_esc_phone_direct_pwm(pending_dir, pending_gval);
} else {
s_esc.cmd_dir = s_esc.pending_dir;
s_esc.target_us = car0107_throttle_to_us(s_esc.pending_dir,
s_esc.pending_gval);
s_esc.current_us = CAR0107_ESC_US_NEUTRAL;
s_esc.trans_phase = CAR0107_TRANS_RAMP_RUN;
car0107_esc_start_ramp(pending_dir, pending_gval);
}
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
}
} else if (s_esc.trans_phase == CAR0107_TRANS_RAMP_RUN) {
s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
s_esc.target_us,
CAR0107_ESC_RAMP_STEP_RUN);
if (s_esc.current_us == s_esc.target_us) {
s_esc.state = (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ?
CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
s_esc.trans_phase = CAR0107_TRANS_BRAKE;
}
}
break;
case CAR0107_ESC_TRANSITION:
s_esc.current_us = car0107_esc_ramp_toward(s_esc.current_us,
s_esc.target_us,
car0107_esc_ramp_step_for_dir(
s_esc.cmd_dir));
if (s_esc.current_us == s_esc.target_us) {
s_esc.state = (s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ?
CAR0107_ESC_FORWARD : CAR0107_ESC_REVERSE;
}
break;
default:
break;
}
if (s_esc.state != CAR0107_ESC_TRANSITION ||
s_esc.trans_phase != CAR0107_TRANS_BRAKE) {
int snap = (s_esc.state == CAR0107_ESC_STOP);
if (s_esc.state != CAR0107_ESC_BRAKE_THEN_NEUTRAL &&
s_esc.state != CAR0107_ESC_BRAKE_THEN_REVERSE) {
int snap = (s_esc.state == CAR0107_ESC_NEUTRAL);
car0107_esc_output_us(s_esc.current_us, snap);
}
}
......@@ -632,15 +940,20 @@ static void car0107_reset_steering_state(void)
static void car0107_reset_esc_state(void)
{
s_esc.state = CAR0107_ESC_STOP;
s_esc.state = CAR0107_ESC_NEUTRAL;
s_esc.trans_phase = CAR0107_TRANS_BRAKE;
s_esc.target_us = CAR0107_ESC_US_NEUTRAL;
s_esc.cmd_dir = 0;
s_esc.pending_dir = 0;
s_esc.pending_gval = 0;
s_esc.brake_dir = 0;
s_esc.brake_ticks = 0;
s_esc.neutral_hold_ticks = 0;
s_esc.brake_until_ms = 0;
s_esc.brake_hold_ticks = 0;
s_esc.neutral_unlock_until_ms = 0;
s_esc.last_drive_dir = 0;
s_esc.last_nonzero_mode = 0;
s_esc.pending_stop_start_ms = 0;
s_esc.last_input_ms = 0;
}
static void car0107_apply_stop(void)
......@@ -661,11 +974,33 @@ static void car0107_stop_speed_now(void)
void car0107_middle_pwm(void)
{
int forward_needs_brake;
/* 0107 初始化和异常停止都保证专用线程存在，持续刷新 1500us 中值。 */
car0107_esc_thread_start();
pthread_mutex_lock(&s_car0107_hw_mutex);
forward_needs_brake =
(s_esc.state == CAR0107_ESC_FORWARD ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE ||
(s_esc.state == CAR0107_ESC_TRANSITION &&
s_esc.cmd_dir == CAR0107_ESC_DIR_FORWARD) ||
s_esc.current_us > CAR0107_ESC_DEADZONE_HI);
car0107_reset_steering_state();
car0107_apply_stop();
if (forward_needs_brake) {
my_zlog_info("car0107 ESC AUTO_STOP FORWARD -> BRAKE_THEN_NEUTRAL (%dus)",
car0107_forward_brake_us());
car0107_esc_begin_forward_brake();
car0107_calculate_L_R(90);
} else {
/* 后退/中位异常停止不需要刹车，直接持续输出1500us。 */
car0107_apply_stop();
}
/* 异常停止不算用户控制，不刷新 idle 保活计时 */
pthread_mutex_unlock(&s_car0107_hw_mutex);
car0107_esc_thread_wake();
}
static void car0107_mode_3_left(int gval)
......@@ -697,10 +1032,76 @@ void car0107_control_change(int *buf)
int mode = buf[1];
int raw_val = car0107_clamp_input(buf[2]);
car0107_esc_thread_start();
pthread_mutex_lock(&g_exit_count_mutex);
g_devcontrol_exit_count = 0;
pthread_mutex_unlock(&g_exit_count_mutex);
if (mode == 1 || mode == 2) {
pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_PHONE;
s_target_speed = 0;
s_current_speed = 0;
s_speed_mode = 0;
s_speed_timeout = 0;
s_steering_timeout = 0;
car0107_notify_control_locked();
s_esc.last_input_ms = get_current_time_millis();
if (mode == 1) {
s_phone_mode1_val = raw_val;
if (car0107_phone_is_active(raw_val)) {
s_phone_last_mode = 1;
}
} else {
s_phone_mode2_val = raw_val;
if (car0107_phone_is_active(raw_val)) {
s_phone_last_mode = 2;
}
}
/* 前进只认 mode1 低值停车；mode2,val=0 是前进伴随空帧，必须忽略。 */
if (mode == 1 && raw_val < CAR0107_ESC_RELEASE_MAX &&
car0107_esc_should_forward_brake()) {
s_phone_mode1_val = 0;
s_phone_mode2_val = 0;
s_phone_last_mode = 0;
my_zlog_info("car0107 ESC FORWARD STOP mode=1 val=%d: BRAKE %dus for %dms",
raw_val, car0107_forward_brake_us(),
CAR0107_ESC_BRAKE_HOLD_MS);
car0107_esc_begin_forward_brake();
pthread_mutex_unlock(&s_car0107_hw_mutex);
car0107_esc_thread_wake();
return;
}
/* 后退自己的 mode2,val=0 才表示后退停止；前进状态下同帧不会进这里。 */
if (mode == 2 && raw_val == 0 &&
(s_esc.state == CAR0107_ESC_REVERSE ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE)) {
s_phone_mode1_val = 0;
s_phone_mode2_val = 0;
s_phone_last_mode = 0;
my_zlog_info("car0107 ESC REVERSE STOP mode=2 val=0 -> NEUTRAL 1500us");
car0107_esc_begin_neutral_stop();
car0107_esc_output_us(CAR0107_ESC_US_NEUTRAL, 0);
pthread_mutex_unlock(&s_car0107_hw_mutex);
car0107_esc_thread_wake();
return;
}
/* MQTT 边沿立刻决策；刹车阶段由电调线程独占 tick，避免同包内结束刹车 */
car0107_phone_apply_merged();
if (s_esc.state != CAR0107_ESC_BRAKE_THEN_NEUTRAL &&
s_esc.state != CAR0107_ESC_BRAKE_THEN_REVERSE) {
car0107_esc_tick();
}
pthread_mutex_unlock(&s_car0107_hw_mutex);
car0107_esc_thread_wake();
return;
}
pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_PHONE;
s_target_speed = 0;
......@@ -710,23 +1111,6 @@ void car0107_control_change(int *buf)
s_steering_timeout = 0;
car0107_notify_control_locked();
if (mode == 1) {
s_phone_last_mode = 1;
s_phone_mode1_val = raw_val;
car0107_phone_apply_merged();
car0107_esc_tick();
pthread_mutex_unlock(&s_car0107_hw_mutex);
return;
}
if (mode == 2) {
s_phone_last_mode = 2;
s_phone_mode2_val = raw_val;
car0107_phone_apply_merged();
car0107_esc_tick();
pthread_mutex_unlock(&s_car0107_hw_mutex);
return;
}
switch (mode) {
case 3:
car0107_mode_3_left(raw_val);
......@@ -843,8 +1227,6 @@ static void car0107_steering_angle_smooth_process(void)
void car0107_steering_smooth_all(void)
{
pthread_mutex_lock(&s_car0107_hw_mutex);
car0107_esc_tick();
car0107_speed_smooth_process();
car0107_steering_angle_smooth_process();
pthread_mutex_unlock(&s_car0107_hw_mutex);
}
......@@ -854,6 +1236,8 @@ void car0107_control_steering_change_process(int *buf)
int mode = buf[1];
int val = buf[2];
car0107_esc_thread_start();
pthread_mutex_lock(&s_car0107_hw_mutex);
s_control_source = CTRL_STEERING;
s_steering_timeout = 200;
......@@ -906,4 +1290,105 @@ void car0107_control_steering_change_process(int *buf)
}
}
pthread_mutex_unlock(&s_car0107_hw_mutex);
car0107_esc_thread_wake();
}
static void car0107_esc_thread_add_timespec_ms(struct timespec *ts, int ms)
{
ts->tv_nsec += (long)ms * 1000000L;
if (ts->tv_nsec >= 1000000000L) {
ts->tv_sec += ts->tv_nsec / 1000000000L;
ts->tv_nsec %= 1000000000L;
}
}
static int car0107_esc_thread_process_once(void)
{
int wait_ms = CAR0107_ESC_THREAD_MS;
pthread_mutex_lock(&s_car0107_hw_mutex);
if (s_control_source == CTRL_STEERING) {
car0107_speed_smooth_process();
}
car0107_esc_tick();
if (s_esc.state == CAR0107_ESC_BRAKE_THEN_NEUTRAL ||
s_esc.state == CAR0107_ESC_BRAKE_THEN_REVERSE) {
wait_ms = CAR0107_ESC_BRAKE_THREAD_MS;
}
pthread_mutex_unlock(&s_car0107_hw_mutex);
return wait_ms;
}
static void *car0107_esc_thread_main(void *arg)
{
(void)arg;
while (s_esc_thread_run) {
int wait_ms = car0107_esc_thread_process_once();
pthread_mutex_lock(&s_esc_wake_mutex);
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
car0107_esc_thread_add_timespec_ms(&ts, wait_ms);
pthread_cond_timedwait(&s_esc_wake_cond, &s_esc_wake_mutex, &ts);
pthread_mutex_unlock(&s_esc_wake_mutex);
}
return NULL;
}
void car0107_esc_thread_wake(void)
{
int started;
pthread_mutex_lock(&s_esc_start_mutex);
started = s_esc_thread_started;
pthread_mutex_unlock(&s_esc_start_mutex);
if (!started) {
return;
}
pthread_mutex_lock(&s_esc_wake_mutex);
pthread_cond_signal(&s_esc_wake_cond);
pthread_mutex_unlock(&s_esc_wake_mutex);
}
void car0107_esc_thread_start(void)
{
pthread_mutex_lock(&s_esc_start_mutex);
if (s_esc_thread_started) {
pthread_mutex_unlock(&s_esc_start_mutex);
return;
}
s_esc_thread_run = 1;
if (pthread_create(&s_esc_thread, NULL, car0107_esc_thread_main, NULL) != 0) {
s_esc_thread_run = 0;
pthread_mutex_unlock(&s_esc_start_mutex);
my_zlog_error("car0107 esc thread create failed");
return;
}
s_esc_thread_started = 1;
pthread_mutex_unlock(&s_esc_start_mutex);
my_zlog_info("car0107 esc thread started (%dms tick, dedicated pthread)",
CAR0107_ESC_THREAD_MS);
}
void car0107_esc_thread_close(void)
{
pthread_t thread;
pthread_mutex_lock(&s_esc_start_mutex);
if (!s_esc_thread_started) {
pthread_mutex_unlock(&s_esc_start_mutex);
return;
}
s_esc_thread_run = 0;
thread = s_esc_thread;
pthread_mutex_unlock(&s_esc_start_mutex);
car0107_esc_thread_wake();
pthread_join(thread, NULL);
pthread_mutex_lock(&s_esc_start_mutex);
s_esc_thread_started = 0;
pthread_mutex_unlock(&s_esc_start_mutex);
my_zlog_info("car0107 esc thread closed");
}
drivers/devicecontrol/car/car0107_control.h
View file @ 6edd4759
......@@ -10,5 +10,8 @@ void car0107_notify_control_activity(void);
long long car0107_last_control_ms(void);
int car0107_esc_is_active(void);
void car0107_steering_set_angle_sync(int angle);
void car0107_esc_thread_start(void);
void car0107_esc_thread_wake(void);
void car0107_esc_thread_close(void);
#endif
drivers/devicecontrol/car/car0107_idle.c
View file @ 6edd4759
......@@ -71,12 +71,10 @@ static void car0107_idle_nudge_steering(int turn_left)
{
int angle = turn_left ? CAR0107_IDLE_STEER_LEFT : CAR0107_IDLE_STEER_RIGHT;
/* 刷新电调中位，避免长时间无 PWM 后接收机休眠 */
pwmWrite(PWM_PIN_SPEED, 75);
/* 速度 PWM 只由持有0107硬件锁的控制路径写，idle线程不直接碰速度。 */
car0107_steering_set_angle_sync(angle);
delay_ms(CAR0107_IDLE_STEER_HOLD_MS);
car0107_steering_set_angle_sync(90);
pwmWrite(PWM_PIN_SPEED, 75);
}
static void car0107_idle_monitor_task(void *arg)
......@@ -128,6 +126,7 @@ void car0107_idle_startup(int device_id)
}
car0107_idle_lock();
s_idle_ctx.shutdown = false;
if (thread_pool_add_task(s_idle_ctx.pool, car0107_idle_monitor_task, NULL) != 0) {
my_zlog_error("car0107 idle monitor task submit failed");
}
......@@ -150,5 +149,7 @@ void car0107_idle_thread_close(void)
s_idle_ctx.pool_ready = false;
}
my_zlog_info("car0107 idle monitor thread pool closed");
car0107_esc_thread_close();
my_zlog_info("car0107 idle monitor and ESC threads closed");
}
modules/webrtcpush/mpp_h264_source.c
View file @ 6edd4759
......@@ -41,6 +41,7 @@ struct MppH264Source {
size_t frame_capacity;
guint keyframe_req;
volatile gboolean pending_idr;
volatile gboolean pending_recover;
gulong enc_sink_probe_id;
guint bus_watch_id;
gchar *sprop_parameter_sets;
......@@ -64,10 +65,13 @@ static gboolean bus_watch_cb(GstBus *bus, GstMessage *msg, gpointer user_data)
GError *err = NULL;
gchar *dbg = NULL;
gst_message_parse_error(msg, &err, &dbg);
my_zlog_error("mpp_h264_source: pipeline error: %s (%s)",
my_zlog_error("mpp_h264_source: pipeline error: %s (%s), recovering",
err ? err->message : "unknown", dbg ? dbg : "");
g_clear_error(&err);
g_free(dbg);
/* ERROR 后 pipeline 进入错误态，appsink 永远拉不到帧 -> 黑屏。
* NULL->PLAYING 重置，让 pull 线程的 recover 路径重新唤醒管道。 */
src->pending_recover = TRUE;
break;
}
case GST_MESSAGE_WARNING: {
......@@ -273,12 +277,11 @@ static void configure_v4l2_src(GstElement *src, gboolean compressed_mjpeg)
return;
g_object_set(src, "do-timestamp", TRUE, NULL);
/*
* MJPEG 压缩流用 mmap；原始 NV12/YUYV 再尝试 DMABUF 零拷贝进 MPP。
* MJPEG 压缩流用 mmap；原始 NV12/YUYV 统一用 mmap（io-mode=2）。
* DMABUF(4) 在非 RK 平台/无 dma-buf 导出的 USB 摄像头上会 negotiation 失败 -> 黑屏。
* mmap 兼容性最好，性能损失可接受（720p@24fps）。
*/
if (compressed_mjpeg)
g_object_set(src, "io-mode", 2, NULL);
else
g_object_set(src, "io-mode", 4, NULL);
g_object_set(src, "io-mode", 2, NULL);
}
static GstElement *make_h264_encoder(gboolean prefer_mpp, gboolean use_test,
......@@ -318,9 +321,9 @@ static GstElement *make_h264_encoder(gboolean prefer_mpp, gboolean use_test,
"bitrate", MPP_VIDEO_START_BPS / 1000,
"key-int-max", MPP_VIDEO_GOP,
"byte-stream", TRUE,
"profile", "main",
"profile", "baseline",
NULL);
my_zlog_info("mpp_h264_source: x264enc software fallback");
my_zlog_info("mpp_h264_source: x264enc software fallback (baseline, matches SDP)");
return enc;
}
......@@ -815,6 +818,10 @@ gboolean mpp_h264_source_pull(MppH264Source *src, gboolean force_idr,
if (!src || !src->appsink || !data || !size)
return FALSE;
/* bus 报 ERROR 后 pipeline 卡死，这里及时恢复避免长期黑屏 */
if (src->pending_recover && mpp_h264_source_recover(src))
src->pending_idr = TRUE;
if (timeout_ms < 1)
timeout_ms = (1000 / WEBRTCPUSH_H264_FPS) + 60;
......@@ -872,6 +879,7 @@ gboolean mpp_h264_source_recover(MppH264Source *src)
my_zlog_warn("mpp_h264_source: recovering pipeline (NULL -> PLAYING)");
src->pending_idr = FALSE;
src->pending_recover = FALSE;
gst_element_set_state(src->pipeline, GST_STATE_NULL);
g_usleep(300000);
ret = gst_element_set_state(src->pipeline, GST_STATE_PLAYING);
......
modules/webrtcpush/rtc_client.c
View file @ 6edd4759
......@@ -363,6 +363,38 @@ static gchar *sdp_patch_h264_answer(const gchar *sdp, const gchar *sprop)
return out;
}
static gchar *sdp_ensure_msid_semantic(const gchar *sdp)
{
const gchar *mline;
if (!sdp || strstr(sdp, "a=msid-semantic:"))
return g_strdup(sdp);
mline = strstr(sdp, "m=");
if (!mline)
return g_strdup(sdp);
{
GString *out = g_string_new_len(sdp, (gsize)(mline - sdp));
g_string_append(out, "a=msid-semantic: WMS car-stream\r\n");
g_string_append(out, mline);
my_zlog_info("libdatachannel: SDP answer patched with msid-semantic");
return g_string_free(out, FALSE);
}
}
static gchar *build_h264_profile_fmtp(RtcClient *client)
{
const gchar *sprop = mpp_h264_source_get_sprop(client->h264_source);
if (sprop && sprop[0])
return g_strdup_printf(
"profile-level-id=%s;packetization-mode=1;level-asymmetry-allowed=1;"
"sprop-parameter-sets=%s",
WEBRTCPUSH_H264_PROFILE_LEVEL_ID, sprop);
return g_strdup_printf(
"profile-level-id=%s;packetization-mode=1;level-asymmetry-allowed=1",
WEBRTCPUSH_H264_PROFILE_LEVEL_ID);
}
static void RTC_API on_local_description(int pc, const char *sdp,
const char *type, void *ptr)
{
......@@ -376,6 +408,11 @@ static void RTC_API on_local_description(int pc, const char *sdp,
return;
sprop = mpp_h264_source_get_sprop(client->h264_source);
patched = sdp_patch_h264_answer(sdp, sprop);
{
gchar *with_msid = sdp_ensure_msid_semantic(patched ? patched : sdp);
g_free(patched);
patched = with_msid;
}
json = build_description_json(client->app, "answer", patched ? patched : sdp);
g_free(patched);
if (!json) {
......@@ -474,42 +511,57 @@ static void RTC_API on_state_change(int pc, rtcState state, void *ptr)
static void RTC_API on_track_open(int track, void *ptr)
{
RtcClient *client = ptr;
gboolean video_open = FALSE;
gboolean audio_open = FALSE;
if (!client)
return;
g_mutex_lock(&client->lock);
if (client->track == track) {
client->track_open = TRUE;
client->source_pts_valid = FALSE;
video_open = TRUE;
}
if (client->audio_track == track) {
client->audio_track_open = TRUE;
client->audio_pts_valid = FALSE;
audio_open = TRUE;
}
g_mutex_unlock(&client->lock);
frame_queue_clear(client);
request_idr(client, TRUE);
if (video_open) {
frame_queue_clear(client);
request_idr(client, TRUE);
my_zlog_info("libdatachannel: H264 track open, requesting IDR");
} else if (audio_open) {
my_zlog_info("libdatachannel: Opus track open");
}
signal_track_ready(client);
my_zlog_info("libdatachannel: track open (video=%d audio=%d), requesting IDR",
client->track == track ? 1 : 0,
client->audio_track == track ? 1 : 0);
}
static void RTC_API on_track_closed(int track, void *ptr)
{
RtcClient *client = ptr;
gboolean video_closed = FALSE;
gboolean audio_closed = FALSE;
if (!client)
return;
g_mutex_lock(&client->lock);
if (client->track == track)
if (client->track == track) {
client->track_open = FALSE;
if (client->audio_track == track)
video_closed = TRUE;
}
if (client->audio_track == track) {
client->audio_track_open = FALSE;
audio_closed = TRUE;
}
g_mutex_unlock(&client->lock);
frame_queue_clear(client);
if (video_closed)
frame_queue_clear(client);
signal_track_ready(client);
my_zlog_warn("libdatachannel: track closed (video=%d audio=%d)",
client->track == track ? 1 : 0,
client->audio_track == track ? 1 : 0);
video_closed ? 1 : 0, audio_closed ? 1 : 0);
}
static void RTC_API on_track_error(int track, const char *error, void *ptr)
......@@ -523,7 +575,7 @@ static void RTC_API on_pli(int track, void *ptr)
{
RtcClient *client = ptr;
int dropped = 0;
if (!client)
if (!client || client->track != track)
return;
if (!request_idr(client, FALSE))
my_zlog_debug("libdatachannel: PLI throttled, skip duplicate IDR request");
......@@ -531,8 +583,7 @@ static void RTC_API on_pli(int track, void *ptr)
client->source_pts_valid = FALSE;
g_mutex_unlock(&client->lock);
g_mutex_lock(&client->send_lock);
if (client->track == track)
dropped = rtcClearPacingQueue(track);
dropped = rtcClearPacingQueue(track);
g_mutex_unlock(&client->send_lock);
frame_queue_clear(client);
my_zlog_info("libdatachannel: PLI received, dropped %d queued RTP packet(s), next is IDR",
......@@ -1130,10 +1181,12 @@ static gboolean setup_track(RtcClient *client, int pc,
track_init.name = "car-video";
track_init.msid = "car-stream";
track_init.trackId = "car-video";
track_init.profile =
"profile-level-id=" WEBRTCPUSH_H264_PROFILE_LEVEL_ID
";packetization-mode=1;level-asymmetry-allowed=1";
track = rtcAddTrackEx(pc, &track_init);
{
gchar *profile = build_h264_profile_fmtp(client);
track_init.profile = profile;
track = rtcAddTrackEx(pc, &track_init);
g_free(profile);
}
if (track < 0)
return FALSE;
......
modules/webrtcpush/ws_signaling.c
View file @ 6edd4759
......@@ -155,6 +155,7 @@ static gboolean idle_offer(gpointer user_data) {
OfferIdle *o = user_data;
AppState *app = o->app;
my_zlog_info("webrtcpush: applying phone offer (libdatachannel)");
if (rtc_client_handle_offer(app, o->sdp) != 0)
my_zlog_error("libdatachannel: failed to apply phone offer");
g_free(o->sdp);
......@@ -230,9 +231,11 @@ static void schedule_offer(AppState *app, const gchar *sdp) {
if (!app->member_id) {
g_free(s_pending_offer_sdp);
s_pending_offer_sdp = g_strdup(sdp);
g_print("Offer received before id; holding SDP\n");
my_zlog_info("webrtcpush: phone offer held (awaiting memberId)");
return;
}
my_zlog_info("webrtcpush: phone offer received (%zu bytes), debouncing",
sdp ? strlen(sdp) : 0);
g_free(s_debounced_offer_sdp);
s_debounced_offer_sdp = g_strdup(sdp);
if (s_debounce_offer_id)
......@@ -333,7 +336,8 @@ static void dispatch_json(const gchar *payload) {
g_free(s_app->member_id);
s_app->member_id = g_strdup(json_node_get_string(mn));
}
g_print("memberId=%s\n", s_app->member_id ? s_app->member_id : "?");
my_zlog_info("webrtcpush: signaling memberId=%s",
s_app->member_id ? s_app->member_id : "?");
if (s_app->device_offerer) {
if (s_app->call_enable) {
gst_webrtc_pipeline_start_offer_retry(s_app);
......@@ -350,10 +354,10 @@ static void dispatch_json(const gchar *payload) {
if (!g_strcmp0(t, "num")) {
if (json_object_has_member(root, "message")) {
gint64 n = json_object_get_int_member(root, "message");
g_print("num peers: %" G_GINT64_FORMAT "\n", n);
my_zlog_info("webrtcpush: signaling num peers=%" G_GINT64_FORMAT, n);
if (n < 2)
g_print(" (waiting for phone: https://jywy.yd-ss.com/client.html?dev=%s)\n",
s_app->dev_room ? s_app->dev_room : "?");
my_zlog_info("webrtcpush: waiting for phone https://jywy.yd-ss.com/client.html?dev=%s",
s_app->dev_room ? s_app->dev_room : "?");
}
g_object_unref(parser);
return;
......@@ -384,7 +388,7 @@ static void dispatch_json(const gchar *payload) {
if (!g_strcmp0(t, "leave")) {
if (rtc_client_is_active(s_app)) {
g_print("peer left: reset libdatachannel peer and wait for a new offer\n");
my_zlog_info("webrtcpush: peer left, reset libdatachannel peer");
g_idle_add(idle_pipeline_teardown, s_app);
}
g_object_unref(parser);
......@@ -461,18 +465,17 @@ static void on_ws_closed(SoupWebsocketConnection *conn, gpointer user_data) {
guint code = conn ? soup_websocket_connection_get_close_code(conn) : 0;
gboolean keep_pipeline = s_app && rtc_client_media_open(s_app);
g_print("WebSocket closed code=%u; reconnecting signaling", code);
if (s_ws) {
g_object_unref(s_ws);
s_ws = NULL;
}
if (!s_ws_exiting && s_app) {
if (keep_pipeline)
g_print(" (keep WebRTC pipeline)\n");
my_zlog_warn("webrtcpush: WebSocket closed code=%u, reconnect (keep media)", code);
else if (rtc_client_is_active(s_app))
g_print(" (reset libdatachannel peer)\n");
my_zlog_warn("webrtcpush: WebSocket closed code=%u, reconnect (reset peer)", code);
else
g_print("\n");
my_zlog_warn("webrtcpush: WebSocket closed code=%u, reconnect", code);
reset_signaling_session(s_app, !keep_pipeline);
if (s_reconnect_id)
g_source_remove(s_reconnect_id);
......@@ -485,7 +488,7 @@ static void on_ws_connected(GObject *source_object, GAsyncResult *res, gpointer
GError *err = NULL;
s_ws = soup_session_websocket_connect_finish(SOUP_SESSION(source_object), res, &err);
if (err) {
g_printerr("WebSocket connect failed: %s\n", err->message);
my_zlog_error("webrtcpush: WebSocket connect failed: %s", err->message);
g_error_free(err);
if (!s_ws_exiting && !s_reconnect_id)
s_reconnect_id = g_timeout_add_seconds(2, reconnect_cb, NULL);
......@@ -493,7 +496,8 @@ static void on_ws_connected(GObject *source_object, GAsyncResult *res, gpointer
}
g_signal_connect(s_ws, "message", G_CALLBACK(on_ws_message), NULL);
g_signal_connect(s_ws, "closed", G_CALLBACK(on_ws_closed), NULL);
g_print("WebSocket connected (libsoup)\n");
my_zlog_info("webrtcpush: WebSocket connected dev=%s",
s_app && s_app->dev_room ? s_app->dev_room : "?");
schedule_tx_flush();
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment