前段时间我本来想解调apc220，这样我可以用portapack接收数传数据，但是这个模块资料太少。
后来我想到其实四轴里也经常用nrf24l01或者低功耗蓝牙来做数传，这两种模块也都是fsk。而且更好的是已经有人写了nrf和btle的解调了，只不过这个人使用的是rtlsdr做接收然后stdout输出给他的解码程序。
https://github.com/omriiluz/NRF24-BTLE-Decoder
我之前就知道这个包，但是一直没有用，因为rtlsdr不支持2.4g，需要一个下变频器变到支持的范围。后来我找到crazyflie（国外知名开源小四轴）官网上有讲解如何用gnuradio把hackrf的数据包给解码器解码的文章。
https://wiki.bitcraze.io/misc:hacks:hackrf#sniffing_nrf24_with_gnu_radio_and_hackrf
我仿照文章的操作发现真的可以用（不过有个缓存延缓写入问题）。
正好近期我又仿照rtl_fm写了个hackrf解调fm的纯c程序（不需要gnuradio）。这样我就可以直接用一个c++程序实现hackrf直接解调btle或者nrf24l01了。
不但如此，我还把蓝牙解调全部写到了一起（把函数调用都去掉了），这样后面加入到portapack中会方便好多。
废话不多说，上代码：
#include <signal.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.h>
#include <pthread.h>
#include <libhackrf/hackrf.h>
#include <iostream>
#include <stdint.h>
#include <time.h>
#include <sys/time.h>
#include <stdbool.h>
#include <inttypes.h>
#define MAXIMUM_BUF_LENGTH (16 * 16384)
/* Global variables */
int32_t g_threshold = 0; // Quantization threshold
uint8_t channel_number = 38;
int skipSamples = 1000;
/* Ring Buffer */
#define RB_SIZE 1000
int rb_head=-1;
int16_t *rb_buf;
using namespace std;
static volatile bool do_exit = false;
hackrf_device *device;
uint32_t freq;
uint32_t hardware_sample_rate;
uint16_t buf16[MAXIMUM_BUF_LENGTH];
int16_t buffer[MAXIMUM_BUF_LENGTH];
int lp_len;
int rate_in;
int16_t result_demod[MAXIMUM_BUF_LENGTH];
int result_demod_len;
int pre_r, pre_j;
void sigint_callback_handler(int signum)
{
cout << "Caught signal" << endl;
do_exit = true;
}
void multiply(int ar, int aj, int br, int bj, int *cr, int *cj)
{
*cr = ar*br - aj*bj;
*cj = aj*br + ar*bj;
}
int polar_discriminant(int ar, int aj, int br, int bj)
{
int cr, cj;
double angle;
multiply(ar, aj, br, -bj, &cr, &cj);
angle = atan2((double)cj, (double)cr);
return (int)(angle / 3.14159 * (1<<14));
}
int rx_callback(hackrf_transfer* transfer)
{
for (int i = 0; i < transfer->valid_length; i++)
{
double sample = (int8_t)(transfer->buffer[i]) + 1;
buf16[i] = (int16_t)sample; //s->buf16[i] = (int16_t)buf[i] - 127; s->buf16[i] -127~128 uint16_t, unsigned for negative?
}
memcpy(buffer, buf16, 2*transfer->valid_length);
lp_len = transfer->valid_length;
//fm demod //rate = 1M
int i, pcm;
pcm = polar_discriminant(buffer[0], buffer[1], pre_r, pre_j);
result_demod[0] = (int16_t)pcm;
for (i = 2; i < (lp_len-1); i += 2)
{
pcm = polar_discriminant(buffer[i], buffer[i+1], buffer[i-2], buffer[i-1]);
result_demod[i/2] = (int16_t)pcm;
}
pre_r = buffer[lp_len - 2];
pre_j = buffer[lp_len - 1];
result_demod_len = lp_len/2;
int i4;
for (i4 = 0; i4 < result_demod_len; i4 += 1)
{
int16_t cursamp = (int16_t) (result_demod[i4]);
rb_head++;
rb_head=(rb_head)%RB_SIZE;
rb_buf[rb_head]=(int)cursamp;
skipSamples = skipSamples - 1;
if (skipSamples<1)
{
int32_t threshold_tmp=0;
for (int c=0;c<8;c++)
{
threshold_tmp = threshold_tmp + (int32_t)rb_buf[(rb_head+c)%RB_SIZE];
}
g_threshold = (int32_t)threshold_tmp/8;
int transitions=0;
if (rb_buf[(rb_head+9)%RB_SIZE] > g_threshold)
{
for (int c=0;c<8;c++)
{
if (rb_buf[(rb_head + c)%RB_SIZE] > rb_buf[(rb_head + c + 1)%RB_SIZE])
transitions = transitions + 1;
}
}
else
{
for (int c=0;c<8;c++)
{
if (rb_buf[(rb_head + c)%RB_SIZE] < rb_buf[(rb_head + c + 1)%RB_SIZE])
transitions = transitions + 1;
}
}
bool packet_detected=false;
if ( transitions==4 && abs(g_threshold)<15500)
{
uint8_t packet_data[500];
int packet_length;
uint32_t packet_crc;
uint32_t calced_crc;
uint64_t packet_addr_l;
uint8_t crc[3];
uint8_t packet_header_arr[2];
/* extract address */
packet_addr_l=0;
for (int i=0;i<4;i++)
{
bool current_bit;
uint8_t byte=0;
for (int c=0;c<8;c++)
{
if (rb_buf[(rb_head + (i+1)*8 + c)%RB_SIZE] > g_threshold)
current_bit = true;
else
current_bit = false;
byte |= current_bit << (7-c);
}
uint8_t byte_temp = (uint8_t) (((byte * 0x0802LU & 0x22110LU) | (byte * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
packet_addr_l|=((uint64_t)byte_temp)<<(8*i);
}
if (freq == 2402e6)
{
channel_number = 37;
}
else if (freq == 2426e6)
{
channel_number = 38;
}
else if (freq = 2480e6)
{
channel_number = 39;
}
/* extract pdu header */
for (int t=0;t<2;t++)
{
bool current_bit;
uint8_t byte=0;
for (int c=0;c<8;c++)
{
if (rb_buf[(rb_head + 5*8+t*8 + c)%RB_SIZE] > g_threshold)
current_bit = true;
else
current_bit = false;
byte |= current_bit << (7-c);
}
packet_header_arr[t] = byte;
}
/* whiten header only so we can extract pdu length */
//BTLEWhiten(packet_header_arr, 2, channel_number);
uint8_t byte_temp2 = (uint8_t) (((channel_number * 0x0802LU & 0x22110LU) | (channel_number * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
uint8_t lfsr_1 = byte_temp2 | 2;
int header_length = 2;
int header_counter = 0;
while(header_length--)
{
for(uint8_t i = 0x80; i; i >>= 1)
{
if(lfsr_1 & 0x80)
{
lfsr_1 ^= 0x11;
(packet_header_arr[header_counter]) ^= i;
}
lfsr_1 <<= 1;
}
header_counter = header_counter + 1;
}
if (packet_addr_l==0x8E89BED6)
{
// Advertisement packet
uint8_t byte_temp3 = (uint8_t) (((packet_header_arr[1] * 0x0802LU & 0x22110LU) | (packet_header_arr[1] * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
packet_length=byte_temp3&0x3F;
}
else
{
packet_length=0;
}
/* extract and whiten pdu+crc */
for (int t=0;t<packet_length+2+3;t++)
{
bool current_bit;
uint8_t byte=0;
for (int c=0;c<8;c++)
{
if (rb_buf[(rb_head + 5*8+t*8 + c)%RB_SIZE] > g_threshold)
current_bit = true;
else
current_bit = false;
byte |= current_bit << (7-c);
}
packet_data[t] = byte;
}
//BTLEWhiten(packet_data, packet_length+2+3, channel_number);
uint8_t byte_temp4 = (uint8_t) (((channel_number * 0x0802LU & 0x22110LU) | (channel_number * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
uint8_t lfsr_2 = byte_temp4 | 2;
int pdu_crc_length = packet_length+2+3;
int pdu_crc_counter = 0;
while(pdu_crc_length--)
{
for(uint8_t i = 0x80; i; i >>= 1)
{
if(lfsr_2 & 0x80)
{
lfsr_2 ^= 0x11;
(packet_data[pdu_crc_counter]) ^= i;
}
lfsr_2 <<= 1;
}
pdu_crc_counter = pdu_crc_counter + 1;
}
if (packet_addr_l==0x8E89BED6)
{ // Advertisement packet
crc[0]=crc[1]=crc[2]=0x55;
}
else
{
crc[0]=crc[1]=crc[2]=0;
}
/* calculate packet crc */
//calced_crc=BTLECrc(packet_data, packet_length+2, crc);
uint8_t v, t, d, crc_length;
uint32_t crc_result=0;
crc_length = packet_length + 2;
int counter = 0;
while(crc_length--)
{
uint8_t byte_temp5 = (uint8_t) (((packet_data[counter] * 0x0802LU & 0x22110LU) | (packet_data[counter] * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
d = byte_temp5;
for(v = 0; v < 8; v++, d >>= 1)
{
t = crc[0] >> 7;
crc[0] <<= 1;
if(crc[1] & 0x80) crc[0] |= 1;
crc[1] <<= 1;
if(crc[2] & 0x80) crc[1] |= 1;
crc[2] <<= 1;
if(t != (d & 1))
{
crc[2] ^= 0x5B;
crc[1] ^= 0x06;
}
}
counter = counter + 1;
}
for (v=0;v<3;v++) crc_result=(crc_result<<8)|crc[v];
calced_crc = crc_result;
packet_crc=0;
for (int c=0;c<3;c++) packet_crc=(packet_crc<<8)|packet_data[packet_length+2+c];
/* BTLE packet found, dump information */
if (packet_crc==calced_crc)
{
//printf("MAC: ");
uint8_t mac_data[6];
int counter = 0;
for (int i = 7; i >= 2; i--)
{
uint8_t byte_temp6 = (uint8_t) (((packet_data[i] * 0x0802LU & 0x22110LU) | (packet_data[i] * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
mac_data[counter] = byte_temp6;
counter = counter + 1;
}
for (int i = 0; i < 6; i++)
{
printf("%02X ", mac_data[i]);
}
printf("\n");
packet_detected = true;
}
else
packet_detected = false;
}
if (packet_detected)
{
skipSamples=20;
}
}
}
return 0;
}
int main(int argc, char **argv)
{
signal(SIGINT, &sigint_callback_handler);
int result;
rb_buf = (int16_t *)malloc(RB_SIZE*2);
freq = 2426e6; // chan 38
//freq = 2402e6; // chan 37
//freq = 2480e6; // chan 39
rate_in = 1000000;
hardware_sample_rate = (uint32_t)(rate_in);
result = hackrf_init();
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_init() failed" << endl;
return EXIT_FAILURE;
}
result = hackrf_open(&device);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_open() failed" << endl;
return EXIT_FAILURE;
}
result = hackrf_set_lna_gain(device, 40);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_set_lna_gain() failed" << endl;
return EXIT_FAILURE;
}
result = hackrf_set_vga_gain(device, 26);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_set_vga_gain() failed" << endl;
return EXIT_FAILURE;
}
/* Set the frequency */
result = hackrf_set_freq(device, freq);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_set_freq() failed" << endl;
return EXIT_FAILURE;
}
/* Set the sample rate */
result = hackrf_set_sample_rate(device, hardware_sample_rate);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_set_sample_rate() failed" << endl;
return EXIT_FAILURE;
}
result = hackrf_set_baseband_filter_bandwidth(device, hardware_sample_rate);
if( result != HACKRF_SUCCESS )
{
cout << "hackrf_baseband_filter_bandwidth_set() failed" << endl;
return EXIT_FAILURE;
}
fprintf(stderr, "Output at %u Hz.\n", rate_in);
usleep(100000);
result = hackrf_start_rx(device, rx_callback, NULL);
while ((hackrf_is_streaming(device) == HACKRF_TRUE) && (do_exit == false))
{
usleep(100000);
}
if (do_exit)
{
fprintf(stderr, "\nUser cancel, exiting...\n");
}
else
{
fprintf(stderr, "\nLibrary error, exiting...\n");
}
result = hackrf_close(device);
if(result != HACKRF_SUCCESS)
{
cout << "hackrf_close() failed" << endl;
}
else
{
cout << "hackrf_close() done" << endl;
}
hackrf_exit();
cout << "hackrf_exit() done" << endl;
return 0;
}
有兴趣的朋友可以试一下。下面是编译和使用方法。
扫描二维码关注公众号，回复：
11810294 查看本文章
g++ hackrf_ble_combined.cpp -o hackrf_ble -lhackrf -pthread
./hackrf_ble接下来我就开始往portapack上搬了。
我参考了afsk_rx和wfm两个程序。
代码可以上我的github portapack repo里看，我就不贴了。
/firmware/application/apps/ui_btle_rx.cpp
/firmware/application/apps/ui_btle_rx.hpp
/firmware/baseband/proc_btlerx.cpp
/firmware/baseband/proc_btlerx.hpp不过我还是会碰到问题，一个是我现在采样率必须是4MHz，其实1MHz就够了，但是现在有个格式类型的问题，我必须用portapack库里的降采样函数来转格式类型，降采样函数最低4倍，我需要1MHz的输出，就必须要有4MHz的输入，这样其实会有其他频点的噪声进来。
还有个问题，我现在跳过了crc校验，mac地址有时候有几位是错的。如果我用crc校验，结果会少好多，但是最后结果的mac地址前三位是对的，后三位也是错的，而且与真实值差别很大。所以这个还有优化空间。
下面两个图片是portapack收到的mac和电脑运行hcitool lescan使用蓝牙芯片搜索的结果对比
下面图片我又改了改界面，更方便观察了，但是你可以看到最下面两行mac地址有一些错误的地方
