32343
843447835d
- IPS200 display menu with real-time status and parameter settings - Camera image processing and line tracking - IMU attitude solution and PID control - WiFi SPI oscilloscope data transmission
394 lines
14 KiB
C
394 lines
14 KiB
C
/*********************************************************************************************************************
|
||
* TC264 Opensourec Library 即（TC264 开源库）是一个基于官方 SDK 接口的第三方开源库
|
||
* Copyright (c) 2022 SEEKFREE 逐飞科技
|
||
*
|
||
* 本文件是 TC264 开源库的一部分
|
||
*
|
||
* TC264 开源库 是免费软件
|
||
* 您可以根据自由软件基金会发布的 GPL（GNU General Public License，即 GNU通用公共许可证）的条款
|
||
* 即 GPL 的第3版（即 GPL3.0）或（您选择的）任何后来的版本，重新发布和/或修改它
|
||
*
|
||
* 本开源库的发布是希望它能发挥作用，但并未对其作任何的保证
|
||
* 甚至没有隐含的适销性或适合特定用途的保证
|
||
* 更多细节请参见 GPL
|
||
*
|
||
* 您应该在收到本开源库的同时收到一份 GPL 的副本
|
||
* 如果没有，请参阅<https://www.gnu.org/licenses/>
|
||
*
|
||
* 额外注明：
|
||
* 本开源库使用 GPL3.0 开源许可证协议 以上许可申明为译文版本
|
||
* 许可申明英文版在 libraries/doc 文件夹下的 GPL3_permission_statement.txt 文件中
|
||
* 许可证副本在 libraries 文件夹下 即该文件夹下的 LICENSE 文件
|
||
* 欢迎各位使用并传播本程序 但修改内容时必须保留逐飞科技的版权声明（即本声明）
|
||
*
|
||
* 文件名称 isr
|
||
* 公司名称 成都逐飞科技有限公司
|
||
* 版本信息 查看 libraries/doc 文件夹内 version 文件 版本说明
|
||
* 开发环境 ADS v1.10.2
|
||
* 适用平台 TC264D
|
||
* 店铺链接 https://seekfree.taobao.com/
|
||
*
|
||
* 修改记录
|
||
* 日期 作者 备注
|
||
* 2022-09-15 pudding first version
|
||
********************************************************************************************************************/
|
||
|
||
#include "isr_config.h"
|
||
#include "isr.h"
|
||
#include "attitude_solution.h"
|
||
#include "pid_control.h"
|
||
#include "pid_tuning.h"
|
||
|
||
// 关于TC系列默认是不支持中断嵌套的，希望支持中断嵌套需要在中断中使用 interrupt_global_enable(0); 来开启中断嵌套
|
||
// 简单的说实际上进入中断后TC系列的硬件会自动调用 interrupt_global_disable(); 就不能响应任何的中断，所以需要我们自行手动调用 interrupt_global_enable(0); 来开启中断的响应
|
||
|
||
|
||
|
||
|
||
|
||
|
||
// 陀螺仪自动校准状态
|
||
// gyro_off 使用 Q8 定点格式 (存储值 = 真实偏移 × 256)
|
||
// 避免 EMA 修正量因 int16 截断而丢失 (<1LSB 的修正会累积在定点小数中)
|
||
volatile uint16 gyro_calib_cnt = 0;
|
||
volatile int32 gyro_sum_x = 0, gyro_sum_y = 0, gyro_sum_z = 0;
|
||
volatile int32 gyro_off_x = 0, gyro_off_y = 0, gyro_off_z = 0; // Q8 定点
|
||
volatile uint8 gyro_calib_ok = 0;
|
||
|
||
// CCU60_CH0: 5ms定时中断 - IMU数据采集
|
||
IFX_INTERRUPT(cc60_pit_ch0_isr, 0, CCU6_0_CH0_ISR_PRIORITY)
|
||
{
|
||
interrupt_global_enable(0);
|
||
pit_clear_flag(CCU60_CH0);
|
||
|
||
imu660rb_get_acc();
|
||
imu660rb_get_gyro();
|
||
|
||
// ---------------------------------------------------------------
|
||
// 阶段0: 预稳定 (前30次=150ms) — 丢弃数据, 等待传感器内部稳定
|
||
// 阶段1: 校准累加 (接下来250次=1.25s) — 求平均零偏
|
||
// 阶段2: 运行 — 减零偏 + 漂移跟踪 + Mahony
|
||
// ---------------------------------------------------------------
|
||
if(gyro_calib_ok == 0)
|
||
{
|
||
gyro_calib_cnt++;
|
||
if(gyro_calib_cnt <= 30)
|
||
{
|
||
// 阶段0: 丢弃, 传感器上电后需要时间稳定
|
||
}
|
||
else
|
||
{
|
||
// 阶段1: 累加
|
||
gyro_sum_x += imu660rb_gyro_x;
|
||
gyro_sum_y += imu660rb_gyro_y;
|
||
gyro_sum_z += imu660rb_gyro_z;
|
||
|
||
if(gyro_calib_cnt >= 280) // 30预稳定 + 250校准 = 1.4秒
|
||
{
|
||
gyro_off_x = ((int32)(gyro_sum_x / 250)) << 8; // Q8定点
|
||
gyro_off_y = ((int32)(gyro_sum_y / 250)) << 8;
|
||
gyro_off_z = ((int32)(gyro_sum_z / 250)) << 8;
|
||
gyro_calib_ok = 1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 阶段2: 扣除零偏 (Q8定点取整数部分)
|
||
{
|
||
int16 off_x = (int16)(gyro_off_x >> 8);
|
||
int16 off_y = (int16)(gyro_off_y >> 8);
|
||
int16 off_z = (int16)(gyro_off_z >> 8);
|
||
imu660rb_gyro_x -= off_x;
|
||
imu660rb_gyro_y -= off_y;
|
||
imu660rb_gyro_z -= off_z;
|
||
}
|
||
|
||
// 阶段3: 静止时跟踪温漂 (Q8定点EMA, 无截断问题)
|
||
// 阈值 100LSB≈7°/s, >>7 时间常数 0.64s
|
||
if(gyro_calib_ok == 1)
|
||
{
|
||
int16 cx = imu660rb_gyro_x;
|
||
int16 cy = imu660rb_gyro_y;
|
||
int16 cz = imu660rb_gyro_z;
|
||
int16 ax = cx, ay = cy, az = cz;
|
||
if(ax < 0) ax = -ax;
|
||
if(ay < 0) ay = -ay;
|
||
if(az < 0) az = -az;
|
||
if(ax < 100 && ay < 100 && az < 100)
|
||
{
|
||
// Q8定点EMA: offset_q8 += (raw - offset_int) * 2
|
||
// 等效于 >>7 但不会因 int16 截断丢失小修正
|
||
int32 raw_x = (int32)cx + (gyro_off_x >> 8);
|
||
int32 raw_y = (int32)cy + (gyro_off_y >> 8);
|
||
int32 raw_z = (int32)cz + (gyro_off_z >> 8);
|
||
gyro_off_x += (raw_x - (gyro_off_x >> 8)) << 1;
|
||
gyro_off_y += (raw_y - (gyro_off_y >> 8)) << 1;
|
||
gyro_off_z += (raw_z - (gyro_off_z >> 8)) << 1;
|
||
}
|
||
}
|
||
|
||
// Mahony互补滤波姿态解算 (校准完成后立即启动)
|
||
if(gyro_calib_ok == 1)
|
||
{
|
||
attitude_update();
|
||
|
||
/* 四轮车无需直立, 只更新陀螺仪 z 轴角速度 (°/s) 供转向环 D 项使用 */
|
||
g_gyro_z_dps = imu660rb_gyro_transition(imu660rb_gyro_z);
|
||
}
|
||
}
|
||
|
||
IFX_INTERRUPT(cc60_pit_ch1_isr, 0, CCU6_0_CH1_ISR_PRIORITY)
|
||
{
|
||
interrupt_global_enable(0);
|
||
pit_clear_flag(CCU60_CH1);
|
||
|
||
/* 此通道暂未使用, 编码器读取已移至 CCU61_CH0 */
|
||
|
||
}
|
||
|
||
IFX_INTERRUPT(cc61_pit_ch0_isr, 0, CCU6_1_CH0_ISR_PRIORITY)
|
||
{
|
||
interrupt_global_enable(0);
|
||
pit_clear_flag(CCU61_CH0);
|
||
|
||
extern volatile int16 encoder_data_1, encoder_data_2;
|
||
|
||
encoder_data_1 = -encoder_get_count(TIM4_ENCODER);
|
||
encoder_clear_count(TIM4_ENCODER);
|
||
encoder_data_2 = encoder_get_count(TIM6_ENCODER);
|
||
encoder_clear_count(TIM6_ENCODER);
|
||
|
||
/* 同步菜单参数 → 真实PID (按键调参生效) */
|
||
pid_tuning_sync();
|
||
|
||
/* 四轮车速度环控制: 左/右编码器速度 → 左/右PWM */
|
||
cascade_speed_loop(encoder_data_1, encoder_data_2);
|
||
|
||
/* 发送关键变量到虚拟示波器 (上位机波形显示) */
|
||
pid_tuning_scope_output();
|
||
}
|
||
|
||
IFX_INTERRUPT(cc61_pit_ch1_isr, 0, CCU6_1_CH1_ISR_PRIORITY)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
pit_clear_flag(CCU61_CH1);
|
||
|
||
|
||
|
||
|
||
}
|
||
// **************************** PIT中断函数 ****************************
|
||
|
||
|
||
// **************************** 外部中断函数 ****************************
|
||
IFX_INTERRUPT(exti_ch0_ch4_isr, 0, EXTI_CH0_CH4_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
if(exti_flag_get(ERU_CH4_REQ8_P33_7)) // 通道4中断
|
||
{
|
||
exti_flag_clear(ERU_CH4_REQ8_P33_7);
|
||
camera_vsync_handler_1(); // 摄像头1 场采集统一回调函数
|
||
}
|
||
|
||
if(exti_flag_get(ERU_CH0_REQ0_P15_4)) // 通道0中断
|
||
{
|
||
exti_flag_clear(ERU_CH0_REQ0_P15_4);
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
// IMU660RB 不使用外部中断引脚，imu660rc_callback 已移除
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
}
|
||
|
||
// 关于摄像头pclk引脚默认占用了 1通道用于给DMA提供时钟 这里不再定义中断函数
|
||
IFX_INTERRUPT(exti_ch1_ch5_isr, 0, EXTI_CH1_CH5_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
if(exti_flag_get(ERU_CH1_REQ10_P14_3)) // 通道1中断
|
||
{
|
||
exti_flag_clear(ERU_CH1_REQ10_P14_3);
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
|
||
|
||
|
||
|
||
}
|
||
|
||
if(exti_flag_get(ERU_CH5_REQ1_P15_8)) // 通道5中断
|
||
{
|
||
exti_flag_clear(ERU_CH5_REQ1_P15_8);
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
|
||
|
||
|
||
|
||
}
|
||
}
|
||
|
||
IFX_INTERRUPT(exti_ch2_ch6_isr, 0, EXTI_CH2_CH6_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
if(exti_flag_get(ERU_CH2_REQ7_P00_4)) // 通道2中断
|
||
{
|
||
exti_flag_clear(ERU_CH2_REQ7_P00_4);
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
|
||
|
||
|
||
}
|
||
if(exti_flag_get(ERU_CH6_REQ9_P20_0)) // 通道6中断
|
||
{
|
||
exti_flag_clear(ERU_CH6_REQ9_P20_0);
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头PCLK 请勿使用摄像头 请不要删除该中断函数
|
||
|
||
|
||
|
||
|
||
}
|
||
}
|
||
|
||
IFX_INTERRUPT(exti_ch3_ch7_isr, 0, EXTI_CH3_CH7_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
if(exti_flag_get(ERU_CH3_REQ3_P10_3)) // 通道3中断
|
||
{
|
||
exti_flag_clear(ERU_CH3_REQ3_P10_3);
|
||
camera_vsync_handler_2(); // 摄像头2场采集统一回调函数
|
||
}
|
||
|
||
if(exti_flag_get(ERU_CH7_REQ16_P15_1)) // 通道7中断
|
||
{
|
||
exti_flag_clear(ERU_CH7_REQ16_P15_1);
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
// 注意: 该外部中断为摄像头X中断 请勿使用摄像头 请不要删除该中断函数
|
||
|
||
|
||
|
||
}
|
||
}
|
||
// **************************** 外部中断函数 ****************************
|
||
|
||
|
||
// **************************** DMA中断函数 ****************************
|
||
IFX_INTERRUPT(dma_ch6_isr, 0, DMA_INT_PRIO_1)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
camera_dma_handler_1(); // 摄像头1 采集完成统一回调函数
|
||
}
|
||
|
||
IFX_INTERRUPT(dma_ch7_isr, 0, DMA_INT_PRIO_2)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
camera_dma_handler_2(); // 摄像头2 采集完成统一回调函数
|
||
}
|
||
// **************************** DMA中断函数 ****************************
|
||
|
||
|
||
// **************************** 串口中断函数 ****************************
|
||
// 串口0默认为调试串口
|
||
IFX_INTERRUPT(uart0_tx_isr, 0, UART0_TX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
|
||
|
||
}
|
||
IFX_INTERRUPT(uart0_rx_isr, 0, UART0_RX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
#if DEBUG_UART_USE_INTERRUPT // 如果使能了 debug 串口中断
|
||
debug_interrupr_handler(); // 处理 debug 串口接收处理函数 数据会被 debug 串口缓存区获取
|
||
#endif // 如果修改了 DEBUG_UART_INDEX 这里的代码需要放到对应的串口中断去
|
||
}
|
||
|
||
|
||
// 串口1默认连接到摄像头配置串口
|
||
IFX_INTERRUPT(uart1_tx_isr, 0, UART1_TX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
|
||
|
||
|
||
}
|
||
IFX_INTERRUPT(uart1_rx_isr, 0, UART1_RX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
camera_uart_handler_1(); // 摄像头配置串口统一回调函数
|
||
}
|
||
|
||
// 串口2默认连接到无线转串口模块
|
||
IFX_INTERRUPT(uart2_tx_isr, 0, UART2_TX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
|
||
|
||
}
|
||
|
||
IFX_INTERRUPT(uart2_rx_isr, 0, UART2_RX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
wireless_module_uart_handler(); // 无线模块统一回调函数
|
||
|
||
|
||
|
||
}
|
||
// 串口3默认连接到GPS定位模块
|
||
IFX_INTERRUPT(uart3_tx_isr, 0, UART3_TX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
|
||
|
||
}
|
||
|
||
IFX_INTERRUPT(uart3_rx_isr, 0, UART3_RX_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
|
||
gnss_uart_callback(); // GNSS串口回调函数
|
||
|
||
|
||
|
||
}
|
||
|
||
// 串口通讯错误中断
|
||
IFX_INTERRUPT(uart0_er_isr, 0, UART0_ER_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
IfxAsclin_Asc_isrError(&uart0_handle);
|
||
}
|
||
IFX_INTERRUPT(uart1_er_isr, 0, UART1_ER_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
IfxAsclin_Asc_isrError(&uart1_handle);
|
||
}
|
||
IFX_INTERRUPT(uart2_er_isr, 0, UART2_ER_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
IfxAsclin_Asc_isrError(&uart2_handle);
|
||
}
|
||
IFX_INTERRUPT(uart3_er_isr, 0, UART3_ER_INT_PRIO)
|
||
{
|
||
interrupt_global_enable(0); // 开启中断嵌套
|
||
IfxAsclin_Asc_isrError(&uart3_handle);
|
||
}
|