void nrf_init() {
// Enable SPI correctly
sspInit(0, sspClockPolarity_Low, sspClockPhase_RisingEdge);
// Enable CS & CE pins
gpioSetDir(RB_SPI_NRF_CS, gpioDirection_Output);
gpioSetPullup(&RB_SPI_NRF_CS_IO, gpioPullupMode_Inactive);
gpioSetDir(RB_NRF_CE, gpioDirection_Output);
gpioSetPullup(&RB_NRF_CE_IO, gpioPullupMode_PullUp);
CE_LOW();
// Setup for nrf24l01+
// power up takes 1.5ms - 3.5ms (depending on crystal)
CS_LOW();
nrf_write_reg(R_CONFIG,
R_CONFIG_PRIM_RX| // Receive mode
R_CONFIG_PWR_UP| // Power on
R_CONFIG_EN_CRC // CRC on, single byte
);
nrf_write_reg(R_EN_AA, 0); // Disable Enhanced ShockBurst;
// Set speed / strength
nrf_write_reg(R_RF_SETUP,DEFAULT_SPEED|R_RF_SETUP_RF_PWR_3);
// Clear MAX_RT, just in case.
nrf_write_reg(R_STATUS,R_STATUS_MAX_RT);
};
void
nrf_config_set (nrfconfig config)
{
int i;
nrf_write_reg (R_SETUP_AW, R_SETUP_AW_5);
nrf_write_reg (R_RF_CH, config->channel);
for (i = 0; i < config->nrmacs; i++)
{
nrf_write_reg (R_RX_PW_P0 + i, config->maclen[i]);
if (i == 0)
{
nrf_write_reg_long (R_RX_ADDR_P0, 5, config->mac0);
}
else if (i == 1)
{
nrf_write_reg_long (R_RX_ADDR_P1, 5, config->mac1);
}
else if (i > 1)
{
nrf_write_reg_long (R_RX_ADDR_P0 + i, 1, config->mac2345 + i - 2);
};
};
nrf_write_reg_long (R_TX_ADDR, 5, config->txmac);
nrf_write_reg (R_EN_RXADDR, (1 << config->nrmacs) - 1);
}
int nrf_rcv_pkt_poll(int maxsize, uint8_t * pkt)
{
uint8_t len;
uint8_t status=0;
for(int i=0;i<maxsize;i++) pkt[i] = 0x00; // Sanity: clear packet buffer
status =nrf_cmd_status(C_NOP);
if((status & R_STATUS_RX_P_NO) == R_STATUS_RX_FIFO_EMPTY){
if( (status & R_STATUS_RX_DR) == R_STATUS_RX_DR){
nrf_write_reg(R_STATUS,R_STATUS_RX_DR);
};
return 0;
};
nrf_read_long(C_R_RX_PL_WID,1,&len);
nrf_write_reg(R_STATUS,R_STATUS_RX_DR);
if(len>32 || len==0){
return -2; // no packet error
};
if(len>maxsize){
return -1; // packet too large
};
nrf_read_pkt(len,pkt);
return len;
};
void nrf_init()
{
//ce
LPC_GPIO2->FIODIR |= (1<<2);
//cs_rad
LPC_GPIO2->FIODIR |= (1<<6);
CE_LOW();
delay_ms(11);
// Setup for nrf24l01+
// power up takes 1.5ms - 3.5ms (depending on crystal)
CS_LOW();
nrf_write_reg(R_CONFIG,
R_CONFIG_PRIM_RX| // Receive mode
R_CONFIG_PWR_UP| // Power on
R_CONFIG_EN_CRC // CRC on, single byte
);
nrf_write_reg(R_EN_AA, 0); // Disable Enhanced ShockBurst;
// Set speed / strength
nrf_write_reg(R_RF_SETUP,DEFAULT_SPEED|R_RF_SETUP_RF_PWR_3);
// Clear MAX_RT, just in case.
nrf_write_reg(R_STATUS,R_STATUS_MAX_RT);
// CS_HIGH();
};
void
nrf_init (void)
{
SPI2_init ();
nrf_reset ();
nrf_write_reg (R_CONFIG,
R_CONFIG_PRIM_RX | R_CONFIG_PWR_UP | R_CONFIG_EN_CRC);
nrf_write_reg (R_EN_AA, 0);
nrf_write_reg (R_RF_SETUP, R_RF_SETUP_DR_2M | R_RF_SETUP_RF_PWR_3);
nrf_write_reg (R_STATUS, R_STATUS_MAX_RT);
}
// High-Level:
void nrf_rcv_pkt_start(void){
nrf_write_reg(R_CONFIG,
R_CONFIG_PRIM_RX| // Receive mode
R_CONFIG_PWR_UP| // Power on
R_CONFIG_EN_CRC // CRC on, single byte
);
nrf_cmd(C_FLUSH_RX);
nrf_write_reg(R_STATUS,0);
CE_HIGH();
};
void nrf_off() {
nrf_write_reg(R_CONFIG,
R_CONFIG_MASK_RX_DR|
R_CONFIG_MASK_TX_DS|
R_CONFIG_MASK_MAX_RT
); // Most important: no R_CONFIG_PWR_UP
};
char nrf_snd_pkt_crc_encr(int size, uint8_t * pkt, uint32_t const key[4]){
if(size > MAX_PKT)
size=MAX_PKT;
nrf_write_reg(R_CONFIG,
R_CONFIG_PWR_UP| // Power on
R_CONFIG_EN_CRC // CRC on, single byte
);
// nrf_write_long(C_W_TX_PAYLOAD,size,pkt);
uint16_t crc=crc16(pkt,size-2);
pkt[size-2]=(crc >>8) & 0xff;
pkt[size-1]=crc & 0xff;
if(key !=NULL)
xxtea_encode_words((uint32_t*)pkt,size/4,key);
CS_LOW();
xmit_spi(C_W_TX_PAYLOAD);
sspSend(0,pkt,size);
CS_HIGH();
CE_HIGH();
delayms(1); // Send it. (only needs >10ys, i think)
CE_LOW();
return nrf_cmd_status(C_NOP);
};
void
nrf_send_frame (uint8_t * frame, int send_mode)
{
int ret;
if (mode != MODE_NOMODE)
return;
UART2PutStr ("sf\n\r");
nrf_write_reg (R_CONFIG, R_CONFIG_PWR_UP | R_CONFIG_EN_CRC);
NRF_CS_LOW ();
mLED_2_On ();
SPI2_xmit (C_W_TX_PAYLOAD);
SPI2_transmit (frame, 32);
mLED_2_Off ();
NRF_CS_HIGH ();
NRF_CE_HIGH ();
while (1)
{
ret = nrf_read_reg (R_FIFO_STATUS);
if (send_mode)
{
if ((ret & R_FIFO_STATUS_TX_EMPTY) == R_FIFO_STATUS_TX_EMPTY)
break;
}
else
{
if ((ret & R_FIFO_STATUS_TX_FULL) == 0)
break;
}
}
NRF_CE_LOW ();
}
void nrf_set_rx_mac(int pipe, int rxlen, int maclen, const uint8_t * mac){
#ifdef SAFE
assert(maclen>=1 || maclen<=5);
assert(rxlen>=1 || rxlen<=32);
assert(pipe>=0 || pipe<=5);
assert(mac!=NULL);
if(pipe>1)
assert(maclen==1);
#endif
nrf_write_reg(R_RX_PW_P0+pipe,rxlen);
nrf_write_reg_long(R_RX_ADDR_P0+pipe,maclen,mac);
nrf_write_reg(R_EN_RXADDR,
nrf_read_reg(R_EN_RXADDR) | (1<<pipe)
);
};
void nrf_disable_pipe(int pipe){
#ifdef SAFE
assert(pipe>=0 || pipe<=5);
#endif
nrf_write_reg(R_EN_RXADDR,
nrf_read_reg(R_EN_RXADDR) & ~(1<<pipe)
);
};
void nrf_set_power(unsigned char mode)
{
nrf_reg_buf buf;
// Power up radio
nrf_read_reg(NRF_REG_CONFIG, &buf);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_PWR_UP, &buf, mode);
nrf_write_reg(NRF_REG_CONFIG, &buf);
delay((10000 * DF) * mode);
}
int
nrf_receive_poll (uint8_t * frame)
{
uint8_t len;
uint8_t status = 0;
if (mode != MODE_RECEIVE_POLL)
return (-1);
status = nrf_cmd_status (C_NOP);
if ((status & R_STATUS_RX_P_NO) == R_STATUS_RX_FIFO_EMPTY)
{
if ((status & R_STATUS_RX_DR) == R_STATUS_RX_DR)
{
nrf_write_reg (R_STATUS, R_STATUS_RX_DR);
};
return 0;
}
nrf_read_long (C_R_RX_PL_WID, 1, &len);
nrf_write_reg (R_STATUS, R_STATUS_RX_DR);
if (len > 32)
return -2;
if (len < 1)
return -1;
/*
UART2PutHex (status);
UART2PutStr (" ");
UART2PutHex (len);
UART2PutStr ("\r\n");
*/
mLED_2_On ();
nrf_read_pkt (32, frame);
mLED_2_Off ();
return len;
}
void
nrf_send_frame (uint8_t * frame)
{
int ret;
nrf_write_reg (R_CONFIG, R_CONFIG_PWR_UP | R_CONFIG_EN_CRC);
for (ret = 0; ret < 32; ret++)
{
UART2PutHex (frame[ret]);
UART2PutStr (" ");
}
UART2PutStr ("\n\r");
NRF_CS_LOW ();
SPI2_xmit (C_W_TX_PAYLOAD);
SPI2_transmit (frame, 32);
NRF_CS_HIGH ();
NRF_CE_HIGH ();
while (1)
{
ret = nrf_read_reg (R_FIFO_STATUS);
if ((ret & R_FIFO_STATUS_TX_EMPTY) == R_FIFO_STATUS_TX_EMPTY)
break;
}
NRF_CE_LOW ();
nrf_write_reg (R_STATUS,
R_CONFIG_MASK_RX_DR | R_CONFIG_MASK_TX_DS |
R_CONFIG_MASK_MAX_RT);
ret = nrf_cmd_status (C_NOP);
}
void
nrf_receive_start (void)
{
if (mode != MODE_NOMODE)
return (-1);
/*
nrf_write_reg (R_CONFIG, R_CONFIG_PRIM_RX |
R_CONFIG_PWR_UP | R_CONFIG_EN_CRC);
*/
// nrf_cmd (C_FLUSH_RX);
nrf_write_reg (R_STATUS, 0);
NRF_CE_HIGH ();
mode = MODE_RECEIVE_POLL;
}
void nrf_set_mode_prx()
{
nrf_reg_buf buf;
nrf_set_power(0); // PWR OFF
nrf_read_reg(NRF_REG_CONFIG, &buf);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_PRIM_RX, &buf, NRF_MODE_PRX);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_MASK_TX_DS, &buf, 1);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_MASK_RX_DR, &buf, 0);
nrf_write_reg(NRF_REG_CONFIG, &buf);
nrf_set_power(1); // PWR ON
}
int nrf_send_blocking(nrf_payload *payload)
{
int i;
nrf_reg_buf status;
nrf_read_reg(NRF_REG_STATUS, &status);
// if TX buffer is full, indicate no bytes where sent, flush TX buffer
if(nrf_get_reg_field(NRF_REG_STATUS, NRF_REGF_TX_FULL, &status) == 1) {
nrf_spi_csl();
nrf_spi_xfer_byte(NRF_CMD_FLUSH_TX);
nrf_spi_csh();
i = NRF_ERR_TX_FULL;
}
// send command
nrf_spi_csl();
nrf_spi_xfer_byte(NRF_CMD_TX);
// send payload
for(i = 0; i < payload->size; i++) {
nrf_spi_xfer_byte(payload->data[i]);
}
nrf_spi_csh();
// wait until payload passed TX FIFO
do {
nrf_read_reg(NRF_REG_STATUS, &status);
// If MAX_RT is reached, indicate no bytes where sent ...
if(nrf_get_reg_field(NRF_REG_STATUS, NRF_REGF_MAX_RT, &status) == 1) {
nrf_spi_csl();
nrf_spi_xfer_byte(NRF_CMD_FLUSH_TX);
nrf_spi_csh();
i = NRF_ERR_MAX_RT;
break;
}
} while(nrf_get_reg_field(NRF_REG_STATUS, NRF_REGF_TX_DS , &status) != 1);
// clear TX_DS/MAX_RT bits (by writing 1 or just writing back the status)
nrf_write_reg(NRF_REG_STATUS, &status);
return i;
}
char snd_pkt_no_crc(int size, uint8_t * pkt)
{
if(size > MAX_PKT)
size=MAX_PKT;
nrf_write_reg(R_CONFIG,
R_CONFIG_PWR_UP| // Power on
R_CONFIG_EN_CRC // CRC on, single byte
);
CS_LOW();
xmit_spi(C_W_TX_PAYLOAD);
sspSend(0,pkt,size);
CS_HIGH();
CE_HIGH();
delayms(1); // Send it. (only needs >10ys, i think)
CE_LOW();
return nrf_cmd_status(C_NOP);
}
int nrf_receive_blocking(nrf_payload *payload)
{
unsigned char i = 0;
nrf_reg_buf status;
// wait until data arrives
do {
nrf_read_reg(NRF_REG_STATUS, &status);
} while(nrf_get_reg_field(NRF_REG_STATUS, NRF_REGF_RX_DR, &status) != 1);
// receive payload
nrf_spi_csl();
nrf_spi_xfer_byte(NRF_CMD_RX);
for(i = 0; i < payload->size; i++) {
payload->data[i] = nrf_spi_xfer_byte(NRF_CMD_RREG);
}
nrf_spi_csh();
// write back status to clean RX_DR
nrf_write_reg(NRF_REG_STATUS, &status);
nrf_read_reg(NRF_REG_FIFO_STATUS, &status);
// if RX buffer is full, indicate no bytes where receifed, flush RX buffer
if(nrf_get_reg_field(NRF_REG_FIFO_STATUS, NRF_REGF_FIFO_RX_FULL, &status) == 1) {
nrf_spi_csl();
nrf_spi_xfer_byte(NRF_CMD_FLUSH_RX);
nrf_spi_csh();
return NRF_ERR_RX_FULL;
}
return i;
}
void nrf_set_strength(unsigned char strength){
if(strength>3)
strength=3;
nrf_write_reg(R_RF_SETUP,DEFAULT_SPEED|(strength<<1));
};
void nrf_set_channel(int channel)
{
nrf_write_reg(R_RF_CH, channel);
};
void nrf_set_rx_mac(int pipe, int rxlen, int maclen, const uint8_t * mac)
{
nrf_write_reg(R_RX_PW_P0+pipe,rxlen);
nrf_write_reg_long(R_RX_ADDR_P0+pipe,maclen,mac);
};
void nrf_rcv_pkt_end(void)
{
CE_LOW();
nrf_cmd(C_FLUSH_RX);
nrf_write_reg(R_STATUS,R_STATUS_RX_DR);
};
void nrf_set_channel(int channel){
#ifdef SAFE
assert(channel &~R_RF_CH_BITS ==0);
#endif
nrf_write_reg(R_RF_CH, channel);
};
void main_bridge(void)
{
GLOBAL(daytrig)=10;
GLOBAL(lcdbacklight)=10;
GLOBAL(privacy) = 3;
char input[64];
char led1=0;
char led2=0;
usbCDCInit();
delayms(500);
nrf_init();
nrf_config_set(&config);
nrf_rcv_pkt_start(R_CONFIG_EN_CRC);
while(1){
int l, i, status;
CDC_OutBufAvailChar (&l);
if(l>0){
gpioSetValue (RB_LED0, led1);led1=1-led1;
CDC_RdOutBuf (input, &l);
for(i=0; i<l; i++){
uint8_t cmd = serialmsg_put(input[i]);
if( cmd != SERIAL_NONE ){
switch( cmd ){
case '1':
// can we loose packets here?
nrf_rcv_pkt_end();
status=snd_pkt_no_crc(serialmsg_len, serialmsg_message);
//status=nrf_snd_pkt_crc(serialmsg_len, serialmsg_message);
nrf_rcv_pkt_start(R_CONFIG_EN_CRC);
break;
case '3':
memcpy(config.txmac, serialmsg_message, 5);
nrf_write_long(C_W_REGISTER|R_TX_ADDR,5,config.txmac);
break;
case '4':
memcpy(config.mac0, serialmsg_message, 5);
nrf_write_long(C_W_REGISTER|R_RX_ADDR_P0,5,config.mac0);
nrf_write_reg(R_EN_RXADDR,1);
break;
case '5':
config.channel=serialmsg_message[0];
nrf_set_channel(config.channel);
nrf_cmd(C_FLUSH_RX);
break;
case '6':
config.maclen[0]=serialmsg_message[0];
nrf_write_reg(R_RX_PW_P0,config.maclen[0]);
break;
case '7':
puts("\\7");
char s[sizeof(uint32_t)+1];
*((uint32_t*)s) =GetUUID32();
s[sizeof(uint32_t)]=0;
puts(s);
puts("\\0");
break;
case '8': /* set mac width */
nrf_write_reg(R_SETUP_AW,serialmsg_message[0]);
break;
case '9': // Dis/Enable CRC
nrf_write_reg(R_CONFIG, R_CONFIG_PRIM_RX|R_CONFIG_PWR_UP|
((serialmsg_message[0]&1)?R_CONFIG_EN_CRC :0)|
((serialmsg_message[0]&2)?R_CONFIG_CRCO :0)
);
/* maybe add enhanced shockburst stuff here */
nrf_cmd(C_FLUSH_RX);
nrf_write_reg(R_STATUS,0);
break;
};
puts("\\2\\0");
}
}
}
int len;
uint8_t buf[32];
len=nrf_rcv_pkt_poll(sizeof(buf),buf);
if( len > 0 ){
gpioSetValue (RB_LED2, led2);led2=1-led2;
puts("\\1");
dump_encoded(len, buf);
puts("\\0");
}
}
}
int main(void)
{
uint8_t buffer[32] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16};
/* Unlock the protected registers */
UNLOCKREG();
/* Enable the 12MHz oscillator oscillation */
DrvSYS_SetOscCtrl(E_SYS_XTL12M, 1);
/* Waiting for 12MHz Xtal stable */
DrvSYS_Delay(5000);
/* HCLK clock source. 0: external 12MHz. */
DrvSYS_SelectHCLKSource(0);
LOCKREG();
DrvSYS_SetClockDivider(E_SYS_HCLK_DIV, 0); /* HCLK clock frequency = HCLK clock source / (HCLK_N + 1) */
nrf_init();
nrf_detect();
//nrf_rx_mode_no_aa(addr,5,16,40);
//nrf_rx_mode(addr,5,16,40);
nrf_rx_mode_dual(addr,5,40);
nrf_test_reg();
DrvGPIO_Open(E_GPA, 2, E_IO_OUTPUT);
DrvGPIO_Open(E_GPA, 3, E_IO_OUTPUT);
DrvGPIO_Open(E_GPA, 4, E_IO_OUTPUT);
DrvGPIO_Open(E_GPA, 5, E_IO_OUTPUT);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while(1) {
uint8_t buffer[32];
if(tx_done) {
static uint8_t yy = 0;
yy++;
if(yy&1) {
DrvGPIO_SetBit(E_GPA,2);
} else {
DrvGPIO_ClrBit(E_GPA,2);
}
if(tx_done == 1) {
} else {
}
buffer[0] = tx_done;
tx_done = 0;
}
if(rx_done) {
static uint8_t xx = 0;
rx_done = 0;
xx++;
if(xx & 1)
DrvGPIO_SetBit(E_GPA,5);
else
DrvGPIO_ClrBit(E_GPA,5);
//nrf_ack_packet(0,buffer, (xx&15) + 1);
nrf_ack_packet(0,rx_buffer, rx_len);
}
}
while(1) {
static uint8_t cnt = 0;
if(cnt&1) {
DrvGPIO_SetBit(E_GPA,2);
} else {
DrvGPIO_ClrBit(E_GPA,2);
}
DrvSYS_Delay(50000*2);
cnt++;
//nrf_tx_packet(buffer, 16);
//buffer[0]++;
if(nrf_rx_packet(buffer,16) == NRF_RX_OK)
{
static uint8_t xx = 0;
xx++;
if(xx & 1)
DrvGPIO_SetBit(E_GPA,5);
else
DrvGPIO_ClrBit(E_GPA,5);
}
}
return 0;
}
void nrf_preset_esbpl(
unsigned char mode, unsigned char rf_ch, unsigned char pw,
unsigned char retr, unsigned char delay, nrf_reg_buf *addr)
{
nrf_reg_buf buf;
// Disable auto ACK on all pipes, except PIPE0
nrf_read_reg(NRF_REG_EN_AA, &buf);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P0, &buf, 1);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P1, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P2, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P3, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P4, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_AA, NRF_REGF_ENAA_P5, &buf, 0);
nrf_write_reg(NRF_REG_EN_AA, &buf);
// Disable RX addresses, except PIPE0
nrf_read_reg(NRF_REG_EN_RXADDR, &buf);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P0, &buf, 1);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P1, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P2, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P3, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P4, &buf, 0);
nrf_set_reg_field(NRF_REG_EN_RXADDR, NRF_REGF_ERX_P5, &buf, 0);
nrf_write_reg(NRF_REG_EN_RXADDR, &buf);
// CONFIG - CRC enable, 2-Bit CRC, RX/TX mode
nrf_read_reg(NRF_REG_CONFIG, &buf);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_EN_CRC, &buf, 1);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_CRCO, &buf, 1);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_PRIM_RX, &buf, mode);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_MASK_MAX_RT, &buf, 1);
if(mode == NRF_MODE_PRX) {
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_MASK_TX_DS, &buf, 1);
} else {
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_MASK_RX_DR, &buf, 1);
}
nrf_write_reg(NRF_REG_CONFIG, &buf);
// Enable auto retry and delay
nrf_read_reg(NRF_REG_SETUP_RETR, &buf);
nrf_set_reg_field(NRF_REG_SETUP_RETR, NRF_REGF_ARC, &buf, retr);
nrf_set_reg_field(NRF_REG_SETUP_RETR, NRF_REGF_ARD, &buf, delay);
nrf_write_reg(NRF_REG_SETUP_RETR, &buf);
// Set address width to 5 bytes
nrf_read_reg(NRF_REG_SETUP_AW, &buf);
nrf_set_reg_field(NRF_REG_SETUP_AW, NRF_REGF_AW, &buf, 0b11);
nrf_write_reg(NRF_REG_SETUP_AW, &buf);
// RX_ADDR_P0 - set receive address data pipe0
nrf_write_reg(NRF_REG_RX_ADDR_P0, addr);
// TX_ADDR - transmit address
nrf_write_reg(NRF_REG_TX_ADDR, addr);
// Set ACK PL + DYN PL
nrf_read_reg(NRF_REG_FEATURE, &buf);
nrf_set_reg_field(NRF_REG_FEATURE, NRF_REGF_EN_DPL, &buf, 1);
nrf_set_reg_field(NRF_REG_FEATURE, NRF_REGF_EN_ACK_PAY, &buf, 1);
nrf_write_reg(NRF_REG_FEATURE, &buf);
// Enable dynamic payload width on PIPE0
nrf_read_reg(NRF_REG_DYNPD, &buf);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P0, &buf, 1);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P1, &buf, 0);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P2, &buf, 0);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P3, &buf, 0);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P4, &buf, 0);
nrf_set_reg_field(NRF_REG_DYNPD, NRF_REGF_DPL_P5, &buf, 0);
nrf_write_reg(NRF_REG_DYNPD, &buf);
if(mode == NRF_MODE_PRX) {
// RX_PW_P0 - set number of bytes in RX payload in data PIPE0
nrf_read_reg(NRF_REG_RX_PW_P0, &buf);
nrf_set_reg_field(NRF_REG_RX_PW_P0, NRF_REGF_PW, &buf, pw);
nrf_write_reg(NRF_REG_RX_PW_P0, &buf);
}
// Set RF-channel
nrf_read_reg(NRF_REG_RF_CH, &buf);
nrf_set_reg_field(NRF_REG_RF_CH, NRF_REGF_RF_CH, &buf, rf_ch);
nrf_write_reg(NRF_REG_RF_CH, &buf);
// Setup Data-Rate to 1MBit and RF power to 0db
nrf_read_reg(NRF_REG_RF_SETUP, &buf);
nrf_set_reg_field(NRF_REG_RF_SETUP, NRF_REGF_RF_DR , &buf, 0);
nrf_set_reg_field(NRF_REG_RF_SETUP, NRF_REGF_RF_PWR, &buf, 3);
nrf_write_reg(NRF_REG_RF_SETUP, &buf);
// Power up radio
nrf_read_reg(NRF_REG_CONFIG, &buf);
nrf_set_reg_field(NRF_REG_CONFIG, NRF_REGF_PWR_UP, &buf, 1);
nrf_write_reg(NRF_REG_CONFIG, &buf);
}
int main(void)
{
/* 2 bit for pre-emption priority, 2 bits for subpriority */
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
setup_systick();
enable_tick_count();
setup_io_leds();
setup_io_usb();
init_sensor_config();
GYRO_INIT();
ACC_INIT();
MAG_INIT();
nrf_init();
nrf_detect();
nrf_rx_mode_dual(nrf_addr, 5, 40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
}
pwm_input_init();
USB_Init();
acc_scale_factor = calc_acc_scale(200);
compute_gyro_runtime_bias(sensors.gyro_rt_bias, 1000);
// wait usb ready
//while ((bDeviceState != CONFIGURED)&&(USBConnectTimeOut != 0))
//{}
current_mode = DT_ATT;
// endless loop
while(1)
{
uint8_t buf[64];
if(frame_100Hz){
frame_100Hz = 0;
buf[0] = 0;
if(current_mode == DT_RCDATA){
prepare_rc_data(buf);
usb_send_data(buf,64);
}else if(current_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
buf[1] = 9;
read_raw_gyro((int16_t*)(buf+2));
read_raw_acc((int16_t*)(buf+8));
read_raw_mag((int16_t*)(buf+14));
usb_send_data(buf,64);
}
if(buf[0]){
usb_send_data(buf,64);
}
}
if(sensor_data_ready){
sensor_data_ready = 0;
if(sensors.sumTime_us){
update_AHRS();
if(current_mode == DT_ATT){
buf[0] = DT_ATT;
buf[1] = 3;
sensors.height = 0.0;
memcpy(buf+2,sensors.attitude,sizeof(sensors.attitude) + 4);
usb_send_data(buf,64);
}
LED4_TOGGLE;
LED5_TOGGLE;
LED10_TOGGLE;
}
// process sensor data
}
if(frame_200Hz){
frame_200Hz = 0;
// if L3GD20 already contains gyro data, rising edge will not occur
if( (current_mode == DT_ATT) && (gyro_hungry>1) ){
if(L3GD20_INT2){
int16_t gyro[3];
read_raw_gyro(gyro);
}
}
if(gyro_hungry < 10)
gyro_hungry++;
}
if(frame_1Hz){
frame_1Hz = 0;
LED3_TOGGLE;
}
}
}
//*****************************************************************************
//
// This example demonstrates how to send a string of data to the UART.
//
//*****************************************************************************
int
main(void)
{
//
// Set the clocking to run directly from the crystal.
//
SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_8MHZ);
//
// Initialize the OLED display and write status.
//
//
// Enable the peripherals used by this example.
//
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
//PC5,PC7 EN,CSN
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE,1<<5|1<<7);
//SPI配置
unsigned long ulDataTx[NUM_SSI_DATA];
unsigned long ulDataRx[NUM_SSI_DATA];
unsigned long ulindex;
unsigned long ultemp=0;
SysCtlPeripheralEnable(SYSCTL_PERIPH_SSI0);
//SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA2_SSI0CLK);
GPIOPinConfigure(GPIO_PA3_SSI0FSS);
GPIOPinConfigure(GPIO_PA4_SSI0RX);
GPIOPinConfigure(GPIO_PA5_SSI0TX);
GPIOPinTypeSSI(GPIO_PORTA_BASE, GPIO_PIN_5 | GPIO_PIN_4 | GPIO_PIN_3 |
GPIO_PIN_2);
SSIConfigSetExpClk(SSI0_BASE, SysCtlClockGet(), SSI_FRF_MOTO_MODE_0,
SSI_MODE_MASTER, 4000000, 8);
/*
GPIODirModeSet(GPIO_PORTA_BASE, GPIO_PIN_3, GPIO_DIR_MODE_OUT);
GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_3, GPIO_STRENGTH_4MA,
GPIO_PIN_TYPE_STD_WPU);
GPIODirModeSet(GPIO_PORTB_BASE, GPIO_PIN_0, GPIO_DIR_MODE_OUT);
GPIOPadConfigSet(GPIO_PORTB_BASE, GPIO_PIN_0 , GPIO_STRENGTH_4MA,
GPIO_PIN_TYPE_STD_WPU);
*/
SSIEnable(SSI0_BASE);
//
// Enable processor interrupts.
//
IntMasterEnable();
//
// Set GPIO A0 and A1 as UART pins.
//
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//
// Configure the UART for 115,200, 8-N-1 operation.
//
UARTConfigSetExpClk(UART0_BASE, SysCtlClockGet(), 115200,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
UART_CONFIG_PAR_NONE));
//
// Enable the UART interrupt.
//
IntEnable(INT_UART0);
UARTIntEnable(UART0_BASE, UART_INT_RX | UART_INT_RT);
//
// Prompt for text to be entered.
//
UARTStdioInit(0);
UARTSend((unsigned char *)"Enter text:\n\r", 12);
UARTSend((unsigned char *)"Enter text:\n\r", 12);
//清零接收缓冲区
while(SSIDataGetNonBlocking(SSI0_BASE, &ulDataRx[0]))
{
}
ulDataTx[0] = 's';
ulDataTx[1] = 'p';
ulDataTx[2] = 'i';
set_nrf24l01_csn_l();
/*
for(ulindex = 0; ulindex < NUM_SSI_DATA; ulindex++)
{
UARTprintf("'%c' ", ulDataTx[ulindex]);
SSIDataPut(SSI0_BASE, ulDataTx[ulindex]);
}
*/
set_nrf24l01_csn_h();
_delay_ms(1);
if( setDataRate( RF24_250KBPS ) )
{
p_variant = true ;
}
//初始化NRF24L01
set_module_tx();
nrf_write_reg(NRF_CONFIG,0x0a);
print_byte_register("CONFIG\t",NRF_CONFIG,1);
init_NRF24L01();
set_module_tx();
unsigned char transfer_value[]="EEWORLD_MSP430_00";
//set_module_tx();
//读不出来spi数据的原因是,原来里面有没读取完的数据,需要先清理,再读写.
setChannel(74);
UARTprintf("getchannel:%d\r\n",getChannel());
// setChannel(24);
// UARTprintf("getchannel:%d\r\n",getChannel());
//写地址
nrf_write_buf(TX_ADDR,(uint8_t*)&addresses[0],5);
uint8_t recvbuf[5];
nrf_read_buf(TX_ADDR,&recvbuf[0],5);
for(int i=0;i<5;i++)
{
UARTprintf("%d:%d ",i,recvbuf[i]);
}
UARTprintf("\r\n");
//end of test write address
uint8_t data[32];
for(int i=0;i<32;i++)
{
data[i]=i;
}
UARTprintf("\r\n");
//while(SSIDataGetNonBlocking(SSI0_BASE, &ulDataRx[0]))
//{
//}
//重新发送前,避免写缓冲区满
flush_tx();
spi_write_reg(STATUS, ( spi_read_reg(STATUS) ) | _BV(MAX_RT) );
//role=role_ping_out
openWritingPipe(addresses[0]);
openReadingPipe(1,addresses[1]);
nrf_write_buf(RX_ADDR_P0,(uint8_t*)&addresses[0],5);
unsigned char test;
//while(1)
{
test=spi_read_reg(0x05);
UARTprintf("test:%d\r\n",test);
_delay_ms(1000);
}
//调试关闭
//nrf_write_reg(EN_AA,0x00);
nrf_write_reg(EN_RXADDR,0x02);
//nrf_write_reg(SETUP_RETR,0x00);
nrf_write_reg(RX_PW_P1,0x20);
//set_module_tx();
nrf_write_reg(NRF_CONFIG,0x0b);
nrf_write_reg(CONFIG, nrf_read_reg(CONFIG) | _BV(PRIM_RX));
nrf_write_reg(STATUS, _BV(RX_DR) | _BV(TX_DS) | _BV(MAX_RT) );
set_nrf24l01_ce_h();
nrf_write_buf(RX_ADDR_P0,(uint8_t*)&addresses[0],5);
set_nrf24l01_ce_h();
if(nrf_read_reg(FEATURE) & _BV(EN_ACK_PAY))
{
flush_tx();
}
flush_rx();
print_status(get_status());
nrf_write_reg(SETUP_AW,0x03);
print_byte_register("SETUP_AW\t",SETUP_AW,1);
print_address_register("RX_ADDR_P0-1",RX_ADDR_P0,2);
print_byte_register("RX_ADDR_P2-5",RX_ADDR_P2,4);
print_address_register("TX_ADDR\t",TX_ADDR,1);
print_byte_register("RX_PW_P0-6",RX_PW_P0,6);
print_byte_register("EN_AA\t",EN_AA,1);
print_byte_register("EN_RXADDR",EN_RXADDR,1);
print_byte_register("RF_CH\t",RF_CH,1);
print_byte_register("RF_SETUP",RF_SETUP,1);
print_byte_register("CONFIG\t",NRF_CONFIG,1);
print_byte_register("DYNPD/FEATURE",DYNPD,2);
UARTprintf("Data Rate\t = %s\r\n", pgm_read_word(&rf24_datarate_e_str_P[getDataRate()]));
UARTprintf("Model\t\t = %s\r\n", pgm_read_word(&rf24_model_e_str_P[isPVariant()]));
UARTprintf("CRC Length\t = %s\r\n",pgm_read_word(&rf24_crclength_e_str_P[getCRCLength()]));
UARTprintf("PA Power\t = %s\r\n", pgm_read_word(&rf24_pa_dbm_e_str_P[getPALevel()]));
Init_Timer_A();
set_nrf24l01_ce_h();
//将业务数据写入:WR_TX_PLOAD
uint8_t fifo_status,status,state,i;
while(1)
{
fifo_status=spi_read_reg(FIFO_STATUS);
if(fifo_status&0x02)
{
status=spi_read_reg(STATUS);
if(status&_BV(RX_DR))
{
state=spi_send_byte(RD_RX_PLOAD);
for(i=0;i<RX_PLOAD_WIDTH;i++)
{
status=spi_send_byte(0xff);
//buf[i]=status;
}
nrf_write_reg(FLUSH_RX,0xFF);
//UARTprintf(".");
counter++;
}
if(status &0x02)
{
nrf_write_reg(FLUSH_RX,0xFF);
//UARTprintf(".");
counter++;
}
nrf_rx_packet(data);
}
}
while(available(0))
{
//UARTprintf(".");
if(nrf_rx_packet(data) == 0)
{
counter++;
}
//UARTprintf(".");
//_delay_ms(50);
/*
set_nrf24l01_ce_l();
nrf_write_buf(WR_TX_PLOAD,data,TX_PLOAD_WIDTH);
set_nrf24l01_ce_h();
_delay_ms(30);
*/
}
}
int main(void)
{
uint32_t currentTime;
// High Speed Telemetry Test Code Begin
char numberString[12];
// High Speed Telemetry Test Code End
RCC_GetClocksFreq(&clocks);
USB_Interrupts_Config();
Set_USBClock();
USB_Init();
// Wait until device configured
//while(bDeviceState != CONFIGURED);
testInit();
LED0_ON;
systemReady = true;
//nrf_tx_mode_no_aa(addr,5,40);
nrf_rx_mode_dual(addr,5,40);
{
uint8_t status = nrf_read_reg(NRF_STATUS);
nrf_write_reg(NRF_WRITE_REG|NRF_STATUS,status); // clear IRQ flags
nrf_write_reg(NRF_FLUSH_RX, 0xff);
nrf_write_reg(NRF_FLUSH_TX, 0xff);
}
while (1)
{
uint8_t buf[64];
static uint8_t last_tx_done = 0;
if(ring_buf_pop(nrf_rx_buffer,buf,32)){
// get data from the adapter
switch(buf[0]){
case SET_ATT:
break;
case SET_MOTOR:
break;
case SET_MODE:
report_mode = buf[1];
break;
}
last_tx_done = 1;
}
if(tx_done){
tx_done = 0;
// report ACK success
last_tx_done = 1;
}
if(ring_buf_pop(nrf_tx_buffer,buf,32)){
if(last_tx_done){
last_tx_done = 0;
nrf_ack_packet(0, buf, 32);
}
}
if (frame_50Hz)
{
int16_t motor_val[4];
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
//memcpy(buf, accelSummedSamples100Hz, 12);
//memcpy(buf+12, gyroSummedSamples100Hz, 12);
//memcpy(buf+24, magSumed, 6);
if(report_mode == DT_ATT){
buf[0] = DT_ATT;
memcpy(buf + 1, &sensors.attitude200Hz[0], 12);
memcpy(buf + 13, &executionTime200Hz, 4);
motor_val[0] = motor[0];
motor_val[1] = motor[1];
motor_val[2] = motor[2];
motor_val[3] = motor[3];
memcpy(buf + 17, motor_val, 8);
usb_send_data(buf , 64);
executionTime50Hz = micros() - currentTime;
}else if(report_mode == DT_SENSOR){
buf[0] = DT_SENSOR;
memcpy(buf + 1, gyroSummedSamples100Hz, 12);
memcpy(buf + 13, accelSummedSamples100Hz, 12);
memcpy(buf + 25, magSumed, 6);
}
//nrf_tx_packet(buf,16);
//if(nrf_rx_packet(buf,16) == NRF_RX_OK)
//{
// LED0_TOGGLE;
//}
ring_buf_push(nrf_tx_buffer, buf, 32);
}
if(frame_10Hz)
{
frame_10Hz = false;
magSumed[XAXIS] = magSum[XAXIS];
magSumed[YAXIS] = magSum[YAXIS];
magSumed[ZAXIS] = magSum[ZAXIS];
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
}
if (frame_100Hz)
{
frame_100Hz = false;
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
}
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
}
systemInit();
systemReady = true;
while (1)
{
///////////////////////////////
if (frame_50Hz)
{
frame_50Hz = false;
currentTime = micros();
deltaTime50Hz = currentTime - previous50HzTime;
previous50HzTime = currentTime;
processFlightCommands();
executionTime50Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_10Hz)
{
LED0_TOGGLE;
frame_10Hz = false;
currentTime = micros();
deltaTime10Hz = currentTime - previous10HzTime;
previous10HzTime = currentTime;
sensors.mag10Hz[XAXIS] = -((float)magSum[XAXIS] / 5.0f * magScaleFactor[XAXIS] - sensorConfig.magBias[XAXIS]);
sensors.mag10Hz[YAXIS] = (float)magSum[YAXIS] / 5.0f * magScaleFactor[YAXIS] - sensorConfig.magBias[YAXIS];
sensors.mag10Hz[ZAXIS] = -((float)magSum[ZAXIS] / 5.0f * magScaleFactor[ZAXIS] - sensorConfig.magBias[ZAXIS]);
magSum[XAXIS] = 0;
magSum[YAXIS] = 0;
magSum[ZAXIS] = 0;
newMagData = true;
pressureAverage = pressureSum / 10;
pressureSum = 0;
calculateTemperature();
calculatePressureAltitude();
sensors.pressureAltitude10Hz = pressureAlt;
serialCom();
if ( EKF_Initialized == false ) EKF_Init( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS] );
executionTime10Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_500Hz)
{
frame_500Hz = false;
currentTime = micros();
deltaTime500Hz = currentTime - previous500HzTime;
previous500HzTime = currentTime;
dt500Hz = (float)deltaTime500Hz * 0.000001f; // For integrations in 500 Hz loop
computeGyroTCBias();
sensors.gyro500Hz[ROLL ] = ((float)gyroSummedSamples500Hz[ROLL] / 2.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[PITCH] = -((float)gyroSummedSamples500Hz[PITCH] / 2.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro500Hz[YAW ] = -((float)gyroSummedSamples500Hz[YAW] / 2.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Predict( sensors.gyro500Hz[ROLL], sensors.gyro500Hz[PITCH], sensors.gyro500Hz[YAW],
dt500Hz );
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
executionTime500Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_200Hz)
{
frame_200Hz = false;
currentTime = micros();
deltaTime200Hz = currentTime - previous200HzTime;
previous200HzTime = currentTime;
dt200Hz = (float)deltaTime200Hz * 0.000001f; // For integrations in 200 Hz loop
#if defined(USE_MADGWICK_AHRS) | defined(USE_MARG_AHRS)
sensors.accel200Hz[XAXIS] = -((float)accelSummedSamples200Hz[XAXIS] / 5.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel200Hz[YAXIS] = -((float)accelSummedSamples200Hz[YAXIS] / 5.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel200Hz[ZAXIS] = -((float)accelSummedSamples200Hz[ZAXIS] / 5.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel200Hz[XAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[XAXIS], &fourthOrder200Hz[AX_FILTER]);
sensors.accel200Hz[YAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[YAXIS], &fourthOrder200Hz[AY_FILTER]);
sensors.accel200Hz[ZAXIS] = computeFourthOrder200Hz(sensors.accel200Hz[ZAXIS], &fourthOrder200Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro200Hz[ROLL ] = ((float)gyroSummedSamples200Hz[ROLL] / 5.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[PITCH] = -((float)gyroSummedSamples200Hz[PITCH] / 5.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro200Hz[YAW ] = -((float)gyroSummedSamples200Hz[YAW] / 5.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#endif
#if defined(USE_MADGWICK_AHRS)
MadgwickAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
#if defined(USE_MARG_AHRS)
MargAHRSupdate( sensors.gyro200Hz[ROLL], sensors.gyro200Hz[PITCH], sensors.gyro200Hz[YAW],
sensors.accel200Hz[XAXIS], sensors.accel200Hz[YAXIS], sensors.accel200Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData,
dt200Hz );
newMagData = false;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
sensors.attitude200Hz[ROLL ] = atan2f( 2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1 - q2q2 + q3q3 );
sensors.attitude200Hz[PITCH] = -asinf( 2.0f * (q1 * q3 - q0 * q2) );
sensors.attitude200Hz[YAW ] = atan2f( 2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1 - q2q2 - q3q3 );
#endif
executionTime200Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_100Hz)
{
frame_100Hz = false;
currentTime = micros();
deltaTime100Hz = currentTime - previous100HzTime;
previous100HzTime = currentTime;
dt100Hz = (float)deltaTime100Hz * 0.000001f; // For integrations in 100 Hz loop
sensors.accel100Hz[XAXIS] = -((float)accelSummedSamples100Hz[XAXIS] / 10.0f - accelRTBias[XAXIS] - sensorConfig.accelBias[XAXIS]) * sensorConfig.accelScaleFactor[XAXIS];
sensors.accel100Hz[YAXIS] = -((float)accelSummedSamples100Hz[YAXIS] / 10.0f - accelRTBias[YAXIS] - sensorConfig.accelBias[YAXIS]) * sensorConfig.accelScaleFactor[YAXIS];
sensors.accel100Hz[ZAXIS] = -((float)accelSummedSamples100Hz[ZAXIS] / 10.0f - accelRTBias[ZAXIS] - sensorConfig.accelBias[ZAXIS]) * sensorConfig.accelScaleFactor[ZAXIS];
sensors.accel100Hz[XAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[XAXIS], &fourthOrder100Hz[AX_FILTER]);
sensors.accel100Hz[YAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[YAXIS], &fourthOrder100Hz[AY_FILTER]);
sensors.accel100Hz[ZAXIS] = computeFourthOrder100Hz(sensors.accel100Hz[ZAXIS], &fourthOrder100Hz[AZ_FILTER]);
computeGyroTCBias();
sensors.gyro100Hz[ROLL ] = ((float)gyroSummedSamples100Hz[ROLL] / 10.0f - gyroRTBias[ROLL ] - gyroTCBias[ROLL ]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[PITCH] = -((float)gyroSummedSamples100Hz[PITCH] / 10.0f - gyroRTBias[PITCH] - gyroTCBias[PITCH]) * GYRO_SCALE_FACTOR;
sensors.gyro100Hz[YAW ] = -((float)gyroSummedSamples100Hz[YAW] / 10.0f - gyroRTBias[YAW ] - gyroTCBias[YAW ]) * GYRO_SCALE_FACTOR;
#if defined(USE_CHR6DM_AHRS)
if ( EKF_Initialized == true ) EKF_Update( sensors.accel100Hz[XAXIS], sensors.accel100Hz[YAXIS], sensors.accel100Hz[ZAXIS],
sensors.mag10Hz[XAXIS], sensors.mag10Hz[YAXIS], sensors.mag10Hz[ZAXIS],
sensorConfig.accelCutoff,
newMagData );
newMagData = false;
sensors.attitude200Hz[ROLL ] = gEstimatedStates.phi;
sensors.attitude200Hz[PITCH] = gEstimatedStates.theta;
sensors.attitude200Hz[YAW ] = gEstimatedStates.psi;
#endif
computeAxisCommands(dt100Hz);
mixTable();
writeServos();
writeMotors();
// High Speed Telemetry Test Code Begin
if ( highSpeedAccelTelemEnabled == true )
{
// 100 Hz Accels
ftoa(sensors.accel100Hz[XAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[YAXIS], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.accel100Hz[ZAXIS], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedGyroTelemEnabled == true )
{
// 100 Hz Gyros
ftoa(sensors.gyro100Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.gyro100Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedRollRateTelemEnabled == true )
{
// Roll Rate, Roll Rate Command
ftoa(sensors.gyro100Hz[ROLL], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[ROLL], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedPitchRateTelemEnabled == true )
{
// Pitch Rate, Pitch Rate Command
ftoa(sensors.gyro100Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[PITCH], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedYawRateTelemEnabled == true )
{
// Yaw Rate, Yaw Rate Command
ftoa(sensors.gyro100Hz[YAW], numberString); uartPrint(numberString); uartPrint(",");
ftoa(rxCommand[YAW], numberString); uartPrint(numberString); uartPrint("\n");
}
if ( highSpeedAttitudeTelemEnabled == true )
{
// 200 Hz Attitudes
ftoa(sensors.attitude200Hz[ROLL ], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[PITCH], numberString); uartPrint(numberString); uartPrint(",");
ftoa(sensors.attitude200Hz[YAW ], numberString); uartPrint(numberString); uartPrint("\n");
}
// High Speed Telemetry Test Code End
executionTime100Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_5Hz)
{
frame_5Hz = false;
currentTime = micros();
deltaTime5Hz = currentTime - previous5HzTime;
previous5HzTime = currentTime;
executionTime5Hz = micros() - currentTime;
}
///////////////////////////////
if (frame_1Hz)
{
frame_1Hz = false;
currentTime = micros();
deltaTime1Hz = currentTime - previous1HzTime;
previous1HzTime = currentTime;
executionTime1Hz = micros() - currentTime;
}
////////////////////////////////
}
}
