优化chassis和ares_sbus模块，改进静态角度判断逻辑 (c3e1c460) · Commits · Wenxi XU / Mambo

drivers/chassis/chassis.c

+4 −3

Original line number	Diff line number	Diff line
		@@ -105,8 +105,8 @@ void cchassis_resolve(chassis_data_t data, const chassis_cfg_t cfg)
		float rollSpeedY = -cfg->pos_X_offset[idx] * data->set_status.gyro;
		float speedX, speedY;
		if (data->static_angle &&
		sqrtf(data->set_status.speedX * data->set_status.speedX +
		data->set_status.speedY * data->set_status.speedY) < 0.001f) {
		(data->set_status.speedX * data->set_status.speedX +
		data->set_status.speedY * data->set_status.speedY) < 0.000001f) {
		int err = wheel_set_static(cfg->wheels[idx], data->angle_to_center[idx]);
		if (err < 0) {
		wheel_set_speed(cfg->wheels[idx], 0, data->angle_to_center[idx]);
		@@ -245,7 +245,8 @@ void chassis_thread_entry(void arg1, void arg2, void *arg3)
		// Total ground acceleration (in chassis frame)
		float ax_total = ax_d + ax_c;
		float ay_total = ay_d + ay_c;
		float a_total_mag = sqrtf(ax_total * ax_total + ay_total * ay_total);
		float a_total_mag;
		arm_sqrt_f32(ax_total * ax_total + ay_total * ay_total, &a_total_mag);

		if (a_total_mag > cfg->max_lin_accel) {
		// Limit the total acceleration

+3 −3

Original line number	Diff line number	Diff line
		@@ -25,8 +25,8 @@ int sbus_parseframe_chan(const struct device *dev, int chan)
		// 检查帧是否为空
		struct sbus_driver_data *data = dev->data;

		int current_cyc = k_cycle_get_32();
		if (k_cyc_to_ms_near32(current_cyc - data->recv_cyc) > 100) {
		int curr_time = k_uptime_get();
		if (curr_time - data->recv_time > 100) {
		return 1024;
		}

		@@ -116,7 +116,7 @@ static void uart_callback(const struct device dev, struct uart_event evt, void
		return;
		}

		data->recv_cyc = k_cycle_get_32();
		data->recv_time = k_uptime_get();
		break;
		}

+1 −1