123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698 |
- #include "driver_bk2425.h"
- #if defined(HAL_RCINPUT_WITH_AP_RADIO) && CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412
- #include <utility>
- #include <AP_HAL_ChibiOS/AP_HAL_ChibiOS.h>
- using namespace ChibiOS;
- extern const AP_HAL::HAL& hal;
- #if (TX_SPEED==250)
- ITX_SPEED Radio_Beken::gTxSpeed = ITX_250;
- #elif (TX_SPEED==100)
- ITX_SPEED Radio_Beken::gTxSpeed = ITX_1000;
- #elif (TX_SPEED==2000)
- ITX_SPEED Radio_Beken::gTxSpeed = ITX_2000;
- #endif
- static const uint8_t Bank1_RegTable[ITX_MAX][IREG_MAX][5]= {
-
- {
- { BK2425_R1_4, 0xf9,0x96,0x8a,0xdb },
- { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 },
- PLL_SPEED,
- { BK2425_R1_13, 0x36,0xb4,0x80,0x00 },
- { BK2425_R1_4, 0xff,0x96,0x8a,0xdb },
- },
-
- {
- { BK2425_R1_4, 0xf9,0x96,0x82,0x1b },
- { BK2425_R1_5, 0x24,0x06,0x0f,0xa6 },
- PLL_SPEED,
- { BK2425_R1_13, 0x36,0xb4,0x80,0x00 },
- { BK2425_R1_4, 0xff,0x96,0x82,0x1b },
- },
-
- {
- { BK2425_R1_4, 0xf9,0x96,0x82,0xdb },
- { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 },
- PLL_SPEED,
- { BK2425_R1_13, 0x36,0xb4,0x80,0x00 },
- { BK2425_R1_4, 0xff,0x96,0x82,0xdb },
- },
-
- {
- { BK2425_R1_4, 0xf9,0x96,0x82,0x21 },
- { BK2425_R1_5, 0x24,0x06,0x0f,0xb6 },
- PLL_SPEED,
- { BK2425_R1_13, 0x36,0xb4,0x80,0x00 },
- { BK2425_R1_4, 0xff,0x96,0x82,0x21 },
- }
- };
- static const uint8_t Bank0_Reg6[ITX_MAX][2] = {
- {BK_RF_SETUP, 0x27},
- {BK_RF_SETUP, 0x07},
- {BK_RF_SETUP, 0x2F},
- {BK_RF_SETUP, 0x37},
- };
- static const uint8_t Bank1_Reg14[]= {
- 0x41,0x20,0x08,0x04,0x81,0x20,0xcf,0xF7,0xfe,0xff,0xff
- };
- static const uint8_t Bank0_Reg[][2]= {
- #if 0
- {BK_CONFIG, BK_CONFIG_PWR_UP | BK_CONFIG_PRIM_RX },
- {BK_EN_AA, 0x00},
- {BK_EN_RXADDR, 0x02},
- {BK_SETUP_AW, 0x03},
- #else
- {BK_CONFIG, BK_CONFIG_EN_CRC | BK_CONFIG_CRCO | BK_CONFIG_PWR_UP | BK_CONFIG_PRIM_RX },
- {BK_EN_AA, 0x00},
- {BK_EN_RXADDR, 0x03},
- {BK_SETUP_AW, 0x03},
- #endif
- {BK_SETUP_RETR, 0x00},
- {BK_RF_CH, 0x17},
-
- #if (TX_SPEED == 2000)
- {BK_RF_SETUP, 0x2F},
- #elif (TX_SPEED == 1000)
- {BK_RF_SETUP, 0x07},
- #elif (TX_SPEED == 250)
- {BK_RF_SETUP, 0x27},
-
- #endif
- {BK_STATUS, 0x07},
- {BK_OBSERVE_TX, 0x00},
- {BK_CD, 0x00},
-
-
- {BK_RX_ADDR_P2, 0xc3},
- {BK_RX_ADDR_P3, 0xc4},
- {BK_RX_ADDR_P4, 0xc5},
- {BK_RX_ADDR_P5, 0xc6},
-
- {BK_RX_PW_P0, PACKET_LENGTH_RX_CTRL},
- {BK_RX_PW_P1, PACKET_LENGTH_RX_BIND},
- {BK_RX_PW_P2, 0x20},
- {BK_RX_PW_P3, 0x20},
- {BK_RX_PW_P4, 0x20},
- {BK_RX_PW_P5, 0x20},
- {BK_FIFO_STATUS,0x00},
-
- {BK_DYNPD, 0x3F},
- {BK_FEATURE, BK_FEATURE_EN_DPL | BK_FEATURE_EN_ACK_PAY | BK_FEATURE_EN_DYN_ACK }
- };
- const uint8_t RegPower[8][2] = {
- { OUTPUT_POWER_REG4_0, OUTPUT_POWER_REG6_0 },
- { OUTPUT_POWER_REG4_1, OUTPUT_POWER_REG6_1 },
- { OUTPUT_POWER_REG4_2, OUTPUT_POWER_REG6_2 },
- { OUTPUT_POWER_REG4_3, OUTPUT_POWER_REG6_3 },
- { OUTPUT_POWER_REG4_4, OUTPUT_POWER_REG6_4 },
- { OUTPUT_POWER_REG4_5, OUTPUT_POWER_REG6_5 },
- { OUTPUT_POWER_REG4_6, OUTPUT_POWER_REG6_6 },
- { OUTPUT_POWER_REG4_7, OUTPUT_POWER_REG6_7 },
- };
- Radio_Beken::Radio_Beken(AP_HAL::OwnPtr<AP_HAL::SPIDevice> _dev) :
- dev(std::move(_dev))
- {
- ResetAddress();
- }
- void Radio_Beken::ResetAddress(void)
- {
-
- TX_Address[0] = 0x33;
- TX_Address[1] = RX0_Address[1] = 0x00;
- TX_Address[2] = RX0_Address[2] = 0x59;
- TX_Address[3] = RX0_Address[3] = 0x00;
- TX_Address[4] = RX0_Address[4] = 0x00;
- RX0_Address[0] = 0x31;
- RX1_Address[0] = 0x32;
- RX1_Address[1] = 0x99;
- RX1_Address[2] = 0x59;
- RX1_Address[3] = 0xC6;
- RX1_Address[4] = 0x2D;
- }
- void Radio_Beken::ReadRegisterMulti(uint8_t address, uint8_t *data, uint8_t len)
- {
- uint8_t tx[len+1];
- uint8_t rx[len+1];
- memset(tx, 0, len+1);
- memset(rx, 0, len+1);
- tx[0] = address;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(tx, rx, len+1);
- DEBUG2_LOW();
- memcpy(data, &rx[1], len);
- }
- void Radio_Beken::WriteRegisterMulti(uint8_t address, const uint8_t *data, uint8_t len)
- {
- uint8_t tx[len+1];
- uint8_t rx[len+1];
- memset(rx, 0, len+1);
- tx[0] = address;
- memcpy(&tx[1], data, len);
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(tx, rx, len+1);
- DEBUG2_LOW();
- }
- uint8_t Radio_Beken::ReadStatus(void)
- {
- uint8_t tx = BK_NOP;
- uint8_t rx = 0;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(&tx, &rx, 1);
- DEBUG2_LOW();
- return rx;
- }
- uint8_t Radio_Beken::ReadReg(uint8_t reg)
- {
- uint8_t tx[2];
- uint8_t rx[2];
- memset(tx, 0, 2);
- memset(rx, 0, 2);
- tx[0] = reg | BK_READ_REG;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(tx, rx, 2);
- DEBUG2_LOW();
- return rx[1];
- }
- uint8_t Radio_Beken::Strobe(uint8_t address)
- {
- uint8_t tx = address;
- uint8_t rx = 0;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(&tx, &rx, 1);
- DEBUG2_LOW();
- return rx;
- }
- void Radio_Beken::SetRBank(uint8_t bank)
- {
- uint8_t lastbank = ReadStatus() & BK_STATUS_RBANK;
- if (!lastbank != !bank) {
- uint8_t tx[2];
- uint8_t rx[2];
- tx[0] = BK_ACTIVATE_CMD;
- tx[1] = 0x53;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(&tx[0], &rx[0], 2);
- DEBUG2_LOW();
- }
- }
- void Radio_Beken::WriteReg(uint8_t address, uint8_t data)
- {
- uint8_t tx[2];
- uint8_t rx[2];
- memset(rx, 0, 2);
- tx[0] = address;
- tx[1] = data;
- DEBUG2_HIGH();
- (void)dev->transfer_fullduplex(tx, rx, 2);
- DEBUG2_LOW();
- }
- void Radio_Beken::WriteRegisterMultiBank1(uint8_t address, const uint8_t *data, uint8_t length)
- {
- SetRBank(1);
- WriteRegisterMulti(address, data, length);
- SetRBank(0);
- }
- void Radio_Beken::SetPower(uint8_t power)
- {
- if (power > 7) {
- power = 7;
- }
- uint8_t oldready = bkReady;
- bkReady = 0;
- hal.scheduler->delay(100);
- SetRBank(1);
- {
- const uint8_t* p = &Bank1_RegTable[fcc.CW_mode ? ITX_CARRIER : gTxSpeed][IREG1_4][0];
- uint8_t idx = *p++;
- uint8_t buf[4];
- buf[0] = *p++;
- buf[1] = *p++;
- buf[2] = *p++;
- buf[3] = *p++;
- buf[0] &= ~0x38;
- buf[0] |= (RegPower[power][0] << 3);
- WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4);
- }
- hal.scheduler->delay(100);
- SetRBank(0);
- hal.scheduler->delay(100);
- uint8_t setup = ReadReg(BK_RF_SETUP);
- setup &= ~(3 << 1);
- setup |= (RegPower[power][1] << 1);
- if (fcc.CW_mode) {
- setup |= 0x10;
- }
- WriteReg(BK_WRITE_REG|BK_RF_SETUP, setup);
- bkReady = oldready;
- fcc.power = power;
- }
- void Radio_Beken::SetChannel(uint8_t freq)
- {
- lastTxChannel = freq;
- WriteReg(BK_WRITE_REG|BK_RF_CH, freq);
- }
- void Radio_Beken::SetCwMode(uint8_t cw)
- {
- uint8_t oldready = bkReady;
- bkReady = 0;
- hal.scheduler->delay(100);
- SetRBank(1);
- {
- const uint8_t* p = &Bank1_RegTable[cw ? ITX_CARRIER : gTxSpeed][IREG1_4][0];
- uint8_t idx = *p++;
- uint8_t buf[4];
- buf[0] = *p++;
- buf[1] = *p++;
- buf[2] = *p++;
- buf[3] = *p++;
- buf[0] &= ~0x38;
- buf[0] |= (RegPower[fcc.power & 7][0] << 3);
- WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4);
- }
- hal.scheduler->delay(100);
- SetRBank(0);
- hal.scheduler->delay(100);
- uint8_t setup = ReadReg(BK_RF_SETUP);
- setup &= ~(3 << 1);
- setup |= (RegPower[fcc.power & 7][1] << 1);
- if (cw) {
- setup |= 0x10;
- }
- WriteReg((BK_WRITE_REG|BK_RF_SETUP), setup);
- fcc.CW_mode = cw != 0;
- bkReady = oldready;
- }
- void Radio_Beken::SetCrcMode(uint8_t disable_crc)
- {
- uint8_t oldready = bkReady;
- bkReady = 0;
- uint8_t config = ReadReg(BK_CONFIG);
- if (disable_crc) {
- config &= ~(BK_CONFIG_EN_CRC | BK_CONFIG_CRCO);
- } else {
- config |= (BK_CONFIG_EN_CRC | BK_CONFIG_CRCO);
- }
- WriteReg((BK_WRITE_REG|BK_CONFIG), config);
- fcc.disable_crc = (disable_crc != 0);
- bkReady = oldready;
- }
- void Radio_Beken::EnableCarrierDetect(bool bEnable)
- {
- if (bEnable == fcc.enable_cd) {
- return;
- }
- uint8_t oldready = bkReady;
- bkReady = 0;
- SetRBank(1);
- {
- const uint8_t* p = &Bank1_RegTable[gTxSpeed][IREG1_5][0];
- uint8_t idx = *p++;
- uint8_t buf[4];
- buf[0] = *p++;
- buf[1] = *p++;
- buf[2] = *p++;
- buf[3] = *p++;
- if (bEnable) {
- buf[1] &= ~0x04;
- }
- WriteRegisterMulti((BK_WRITE_REG|idx), buf, 4);
- }
- SetRBank(0);
- bkReady = oldready;
- fcc.enable_cd = bEnable;
- }
- bool Radio_Beken::CarrierDetect(void)
- {
- if (fcc.enable_cd) {
- if (ReadReg(BK_CD) & 0x01) {
- return true;
- }
- }
- return false;
- }
- void Radio_Beken::SetFactoryMode(uint8_t factory)
- {
- uint8_t oldready = bkReady;
- bkReady = 0;
-
- if (factory) {
-
- TX_Address[0] = 0x35;
- TX_Address[1] = RX1_Address[1] = RX0_Address[1] = 0x99;
- TX_Address[2] = RX1_Address[2] = RX0_Address[2] = 0x59;
- TX_Address[3] = RX1_Address[3] = RX0_Address[3] = 0xC6;
- TX_Address[4] = RX1_Address[4] = RX0_Address[4] = factory;
- RX0_Address[0] = 0x34;
- RX1_Address[0] = 0x43;
- } else {
-
- ResetAddress();
- }
-
- WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0), RX0_Address, 5);
- WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P1), RX1_Address, 5);
- WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR), TX_Address, 5);
- WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03);
-
- fcc.factory_mode = factory;
- bkReady = oldready;
- }
- bool Radio_Beken::Reset(void)
- {
-
- hal.scheduler->delay_microseconds(1000);
- return 0;
- }
- void Radio_Beken::DelayCE(void)
- {
- DEBUG1_LOW();
- hal.scheduler->delay_microseconds(50);
- DEBUG1_HIGH();
- }
- bool Radio_Beken::WasTxMode(void)
- {
-
- return bkMode == BKRADIO_TX;
- }
- bool Radio_Beken::WasRxMode(void)
- {
-
- return bkMode == BKRADIO_RX;
- }
- void Radio_Beken::SwitchToRxMode(void)
- {
- uint8_t value;
- Strobe(BK_FLUSH_RX);
- value = ReadStatus();
- WriteReg(BK_WRITE_REG|BK_STATUS, value);
- BEKEN_CE_LOW();
- DelayCE();
- value = ReadReg(BK_CONFIG);
- value |= BK_CONFIG_PRIM_RX;
- value |= BK_CONFIG_PWR_UP;
- WriteReg(BK_WRITE_REG | BK_CONFIG, value);
- BEKEN_CE_HIGH();
-
- bkMode = BKRADIO_RX;
- }
- void Radio_Beken::SwitchToTxMode(void)
- {
- uint8_t value;
- Strobe(BK_FLUSH_TX);
- Strobe(BK_FLUSH_RX);
-
- BEKEN_CE_LOW();
- DelayCE();
- value = ReadReg(BK_CONFIG);
- value &= ~BK_CONFIG_PRIM_RX;
- value |= BK_CONFIG_PWR_UP;
- WriteReg(BK_WRITE_REG | BK_CONFIG, value);
-
- bkMode = BKRADIO_TX;
- }
- void Radio_Beken::SwitchToIdleMode(void)
- {
- Strobe(BK_FLUSH_TX);
- Strobe(BK_FLUSH_RX);
- BEKEN_PA_LOW();
- BEKEN_CE_LOW();
- DelayCE();
- bkMode = BKRADIO_IDLE;
- }
- void Radio_Beken::SwitchToSleepMode(void)
- {
- uint8_t value;
- Strobe(BK_FLUSH_RX);
- Strobe(BK_FLUSH_TX);
- value = ReadStatus();
- WriteReg(BK_WRITE_REG|BK_STATUS, value);
- BEKEN_PA_LOW();
- BEKEN_CE_LOW();
- DelayCE();
- value = ReadReg(BK_CONFIG);
- value |= BK_CONFIG_PRIM_RX;
- value &= ~BK_CONFIG_PWR_UP;
- WriteReg(BK_WRITE_REG | BK_CONFIG, value);
-
- BEKEN_CE_LOW();
- bkMode = BKRADIO_SLEEP;
- }
- void Radio_Beken::InitBank0Registers(ITX_SPEED spd)
- {
- int8_t i;
-
- for (i=20; i >= 0; i--) {
- uint8_t idx = Bank0_Reg[i][0];
- uint8_t value = Bank0_Reg[i][1];
- if (idx == BK_RF_SETUP) {
- value = Bank0_Reg6[spd][1];
- }
- WriteReg((BK_WRITE_REG|idx), value);
- }
-
- i = ReadReg(BK_FEATURE);
- if (i == 0) {
- WriteReg(BK_ACTIVATE_CMD,0x73);
- }
- for (i = 22; i >= 21; i--) {
- WriteReg((BK_WRITE_REG|Bank0_Reg[i][0]),Bank0_Reg[i][1]);
- }
-
- WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0),RX0_Address,5);
- WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P1),RX1_Address,5);
- WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR),TX_Address,5);
- WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03);
- }
- void Radio_Beken::InitBank1Registers(ITX_SPEED spd)
- {
- int16_t i;
- for (i = IREG1_4; i <= IREG1_13; i++) {
- const uint8_t* p = &Bank1_RegTable[spd][i][0];
- uint8_t idx = *p++;
- WriteRegisterMulti((BK_WRITE_REG|idx), p, 4);
- }
- WriteRegisterMulti((BK_WRITE_REG|BK2425_R1_14),&(Bank1_Reg14[0]),11);
-
- {
- const uint8_t* p = &Bank1_RegTable[spd][IREG1_4A][0];
- uint8_t idx = *p++;
- WriteRegisterMulti((BK_WRITE_REG|idx), p, 4);
- }
- {
- const uint8_t* p = &Bank1_RegTable[spd][IREG1_4][0];
- uint8_t idx = *p++;
- WriteRegisterMulti((BK_WRITE_REG|idx), p, 4);
- }
- }
- void Radio_Beken::SetAddresses(const uint8_t* txaddr)
- {
- TX_Address[1] = RX0_Address[1] = txaddr[1];
- TX_Address[3] = RX0_Address[3] = txaddr[3];
- TX_Address[4] = RX0_Address[4] = txaddr[4];
- WriteRegisterMulti((BK_WRITE_REG|BK_RX_ADDR_P0), RX0_Address, 5);
- WriteRegisterMulti((BK_WRITE_REG|BK_TX_ADDR), TX_Address, 5);
- WriteReg(BK_WRITE_REG|BK_EN_RXADDR, 0x03);
- }
- bool Radio_Beken::ClearAckOverflow(void)
- {
- uint8_t status = ReadStatus();
- if ((BK_STATUS_MAX_RT & status) == 0) {
- return false;
- } else {
- WriteReg((BK_WRITE_REG|BK_STATUS), BK_STATUS_MAX_RT);
- return true;
- }
- }
- bool Radio_Beken::SendPacket(uint8_t type,
- const uint8_t* pbuf,
- uint8_t len)
- {
- uint8_t fifo_sta = ReadReg(BK_FIFO_STATUS);
- bool returnValue = ClearAckOverflow();
- if (!(fifo_sta & BK_FIFO_STATUS_TX_FULL)) {
- numTxPackets++;
- WriteRegisterMulti(type, pbuf, len);
- BEKEN_CE_HIGH();
- }
- return returnValue;
- }
- void Radio_Beken::DumpRegisters(void)
- {
- uint8_t i;
- for (i = 0; i <= BK_FEATURE; ++i) {
- uint8_t len = 1;
- switch (i) {
- case 10: case 11: case 16: len = 5; break;
- case 24: case 25: case 26: case 27: len = 0; break;
- default: len = 1; break;
- };
- if (len == 1) {
-
- } else if (len == 5) {
- uint8_t data[5];
- ReadRegisterMulti(i, &data[0], len);
-
- }
- }
- SetRBank(1);
- for (i = IREG1_4; i <= IREG1_13; ++i) {
- uint8_t len = 4;
- uint8_t data[4];
- ReadRegisterMulti(i, &data[0], len);
-
-
- }
- SetRBank(0);
- }
- #endif
|