195
社区成员
发帖
与我相关
我的任务
分享#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdbool.h>
#include <sys/ioctl.h>
#include <linux/spi/spidev.h>
#include <gpiod.h>
#include <time.h>
// 校验和计算函数
uint8_t calc_check8(uint8_t *h){
uint8_t sum=h[0]+h[2]+h[3]+h[4]+h[5];
return ~sum;
}
uint32_t calc_check32(uint8_t *cmd,uint32_t len){
uint32_t wd=len/4;
uint32_t sum=0;
for(int i=0;i<wd;i++){
sum+=((cmd[i*4]<<24)|(cmd[i*4+1]<<16)|(cmd[i*4+2]<<8)|cmd[i*4+3]);
}
return ~sum;
}
// SPI传输函数
int spi_transmit(int spi,struct spi_ioc_transfer *spi_trans,uint8_t *tx,uint8_t *rx,int len,uint8_t mode){
int status=-1;
if(mode==0){
spi_trans->tx_buf=(unsigned long)tx;
spi_trans->rx_buf=(unsigned long)rx;
spi_trans->len=len;
status=ioctl(spi,SPI_IOC_MESSAGE(1),spi_trans);
}else if(mode==1){
spi_trans->tx_buf=(unsigned long)tx;
spi_trans->rx_buf=0;
spi_trans->len=len;
status=ioctl(spi,SPI_IOC_MESSAGE(1),spi_trans);
}else if(mode==2){
spi_trans->tx_buf=0;
spi_trans->rx_buf=(unsigned long)rx;
spi_trans->len=len;
status=ioctl(spi,SPI_IOC_MESSAGE(1),spi_trans);
}
return status;
}
// 保存并打印ADC数据(仅原始二进制输出到stdout,调试信息到stderr)
void save_and_print_adc(uint8_t *miso, int frame_idx, int adc_len) {
// 跳过前6字节Header,指向原始ADC数据
uint8_t *adc_raw = miso + 6;
// ✅ 只有原始二进制数据输出到stdout(Python通过管道读取)
fwrite(adc_raw, 1, adc_len, stdout);
fflush(stdout); // 强制刷新缓冲区,确保数据立即发送给Python
// ✅ 所有调试信息全部输出到stderr(终端可见,不影响数据流)
fprintf(stderr, "✅ 第 %d 帧采集完成,长度: %d 字节\n", frame_idx, adc_len);
}
// 获取单个chirp数据
int get_one_chirp(int spi,struct spi_ioc_transfer *spi_trans,uint8_t *miso,struct gpiod_line *line,int frame_idx){
uint8_t poll_cmd[14]={0xA5,0x35,0x15,0x0C,0x04,0x00,0x02,0x00,0x2D,0x00,0xE8,0xF3,0xCE,0xFF};
int len=14,status=0,value=0,n=0;
status=spi_transmit(spi,spi_trans,poll_cmd,NULL,len,1);
usleep(2000);
bool more=false;
uint8_t header[6];
uint32_t checksum=0;
for(int i=0;i<poll_cmd[8];i++){
value=0;
while(value!=1){value=gpiod_line_get_value(line);}
status=spi_transmit(spi,spi_trans,NULL,header,6,2);
if(header[0]==0xA5){
n+=1;
}else{
continue;
}
len=((header[5]&0x0F)<<8)|header[4]+4;
more=((header[2]&0x20)==0x20)?true:false;
usleep(200);
status=spi_transmit(spi,spi_trans,NULL,miso,len,2);
// ✅ 所有printf改为fprintf(stderr, ...)
fprintf(stderr, "i:%d,header:",i);
for(int i=0;i<6;i++){fprintf(stderr, "%02X ",header[i]);}
fprintf(stderr, "\n");
// ❌ 注释掉十六进制数据打印(否则会输出大量文本到stdout,污染数据流)
// for(int i=0;i<256;i++){fprintf(stderr, "%02X ",miso[i]);}
// fprintf(stderr, "\n");
// ✅ 修复长度负数bug
int adc_len = len - 6;
if (adc_len < 0) adc_len = 0;
// 保存并打印ADC数据
save_and_print_adc(miso, frame_idx, adc_len);
value=gpiod_line_get_value(line);
if(more==1&&value==0){return 0;}
if(more!=1){
usleep(50);
value=gpiod_line_get_value(line);
if(value==1){
poll_cmd[8]=45-i-1;
checksum=calc_check32(&poll_cmd[2],8);
poll_cmd[10]=checksum>>24;
poll_cmd[11]=(checksum>>16)&0xFF;
poll_cmd[12]=(checksum>>8)&0xFF;
poll_cmd[13]=(checksum&0xFF);
status=spi_transmit(spi,spi_trans,poll_cmd,NULL,14,1);
usleep(2000);
}else{
return n;
}
}
}
return n;
}
// 主函数(无限循环采集,持续输出数据给Python)
int main(){
struct gpiod_chip *chip;
struct gpiod_line *line;
int value=0;
chip=gpiod_chip_open_by_name("gpiochip1");
if(!chip){fprintf(stderr, "open gpio failed\n");return -1;}
line=gpiod_chip_get_line(chip,1);
if(!line){fprintf(stderr, "get line failed\n");gpiod_chip_close(chip);return -1;}
if(gpiod_line_request_input(line,"RADAR_NOTIFY")<0){
fprintf(stderr, "failed to request input\n");
gpiod_line_release(line);
gpiod_chip_close(chip);
return -1;
}
// SPI初始化
int spi=-1;
spi=open("/dev/spidev1.0",O_RDWR);
if(spi<0){
fprintf(stderr, "open device failed\n");
return -1;
}
fprintf(stderr, "open device success,spi=%d\n",spi);
uint8_t mode=SPI_MODE_0;
uint8_t lsb=0x00;
uint8_t bits=8;
uint32_t speed=50000000;
fprintf(stderr, "mode:%d,lsb:%d\n",mode,lsb);
if(ioctl(spi,SPI_IOC_WR_MODE,&mode)<0){close(spi);gpiod_line_release(line);gpiod_chip_close(chip);return -1;}
if(ioctl(spi,SPI_IOC_WR_LSB_FIRST,&lsb)<0){close(spi);gpiod_line_release(line);gpiod_chip_close(chip);return -1;}
if(ioctl(spi,SPI_IOC_WR_BITS_PER_WORD,&bits)<0){close(spi);gpiod_line_release(line);gpiod_chip_close(chip);return -1;}
if(ioctl(spi,SPI_IOC_WR_MAX_SPEED_HZ,&speed)<0){close(spi);gpiod_line_release(line);gpiod_chip_close(chip);return -1;}
fprintf(stderr, "spi initialized\n");
// 唤醒雷达
uint8_t *mosi=(uint8_t *)malloc(sizeof(uint8_t)*4);
uint8_t *miso=(uint8_t *)malloc(sizeof(uint8_t)*4);
fprintf(stderr, "trying to awake radar\n");
struct spi_ioc_transfer spi_trans;
int status=-1;
int len=0;
memset(&spi_trans,0,sizeof(spi_trans));
spi_trans.speed_hz=50000000;
spi_trans.delay_usecs=0;
spi_trans.bits_per_word=8;
bool awake=false;
for(int i=0;i<10;i++){
mosi[0]=0x55;
mosi[1]=0xFF;
mosi[2]=0x55;
mosi[3]=0xFF;
len=4;
status=spi_transmit(spi,&spi_trans,mosi,miso,len,0);
for(int i=0;i<len;i++){
if(miso[i]==0x79){fprintf(stderr, "radar has awaked:%02X\n",miso[i]);awake=true;break;}
}
if(awake){break;}
usleep(1000);
}
if(!awake){
fprintf(stderr, "radar hasn't awaked\n");
gpiod_line_release(line);
gpiod_chip_close(chip);
close(spi);
return -1;
}
usleep(5000);
free(mosi);
free(miso);
// 开始无限循环采集(持续输出数据给Python)
int n=0,m=0;
len=4096;
miso=(uint8_t*)malloc(sizeof(uint8_t)*len);
struct timespec start,end;
clock_gettime(CLOCK_MONOTONIC,&start);
int frame_idx = 0;
// ✅ 改为无限循环,持续采集
while(1){
n += get_one_chirp(spi,&spi_trans,miso,line, frame_idx);
frame_idx++;
value=0;
while(value!=1){value=gpiod_line_get_value(line);}
}
// 资源释放(理论上不会执行到这里)
free(miso);
gpiod_line_release(line);
gpiod_chip_close(chip);
close(spi);
return 0;
}