// Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally
bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
{
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH);
delay(1);
_spi_write(CMD_MS5611_RESET);
delay(4);
// We read the factory calibration
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros();
_state = 1;
Temp=0;
Press=0;
scheduler->register_process( AP_Baro_MS5611::_update );
healthy = true;
return true;
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
if (tnow - _timer < 10000) {
return; // wait for more than 10ms
}
_timer = tnow;
if (_state == 1) {
_s_D2 = _spi_read_adc(); // On state 1 we read temp
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else if (_state == 5) {
_s_D1 = _spi_read_adc();
_state = 1; // Start again from state = 1
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
_updated = true; // New pressure reading
} else {
_s_D1 = _spi_read_adc();
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
_updated = true; // New pressure reading
}
}
void CX10::_spi_write_address(uint8_t address, uint8_t data) {
CS_off;
_spi_write(address);
NOP();
_spi_write(data);
CS_on;
}
// Read the sensor. This is a state machine
// We read one time Temperature (state=1) and then 4 times Pressure (states 2-5)
// temperature does not change so quickly...
void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
_timer = tnow;
if (_state == 1) {
_s_D2 = _spi_read_adc(); // On state 1 we read temp
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
} else if (_state == 5) {
_s_D1 = _spi_read_adc();
_state = 1; // Start again from state = 1
_spi_write(CMD_CONVERT_D2_OSR4096); // Command to read temperature
_updated = true; // New pressure reading
} else {
_s_D1 = _spi_read_adc();
_state++;
_spi_write(CMD_CONVERT_D1_OSR4096); // Command to read pressure
_updated = true; // New pressure reading
}
}
static void optical_write(uint8_t addr, uint8_t data) {
OPTICAL_CSEL_PORT &= ~(1<<OPTICAL_CSEL_BIT);
_delay_us(IO_DELAY);
_spi_write(addr | 1<<7);
_delay_us(IO_DELAY);
_spi_write(data);
_delay_us(IO_DELAY);
OPTICAL_CSEL_PORT |= 1<<OPTICAL_CSEL_BIT;
}
// Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally
bool AP_Baro_MS5611::init()
{
_spi = hal.spi->device(AP_HAL::SPIDevice_MS5611);
if (_spi == NULL) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 could not get "
"valid SPI device driver!"));
return false;
}
_spi_sem = _spi->get_semaphore();
hal.scheduler->suspend_timer_procs();
_spi_write(CMD_MS5611_RESET);
hal.scheduler->delay(4);
// We read the factory calibration
// The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096);
_timer = hal.scheduler->micros();
_state = 0;
Temp=0;
Press=0;
_s_D1 = 0;
_s_D2 = 0;
_d1_count = 0;
_d2_count = 0;
hal.scheduler->register_timer_process( AP_Baro_MS5611::_update );
hal.scheduler->resume_timer_procs();
// wait for at least one value to be read
uint32_t tstart = hal.scheduler->millis();
while (!_updated) {
hal.scheduler->delay(10);
if (hal.scheduler->millis() - tstart > 1000) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro_MS5611 took more than "
"1000ms to initialize"));
healthy = false;
return false;
}
}
healthy = true;
return true;
}
void NRF_transmit_data(BYTE *pBuffer, int nLen)
{
int i;
NRF_Select(TRUE);
//Clear previous ints
NRF_WriteRegister(0x27, 0x7E);
//PWR_UP=1
NRF_WriteRegister(0x20, 0x3A);
delay_ms(2); //Takes 1.5ms from PowerDown to Standby mode
//Clear TX fifo
NRF_WriteCommand(0xE1);
//7 bytes payload
NRF_WriteCommand(0xA0);
//Store payload
for(i=0; i<nLen; i++)
{
_spi_write(pBuffer[i]);
}
NRF_Select(FALSE);
//Pulse CE to start transmission
delay_ms(1);
NRF_Enable(TRUE);
delay_ms(1);
NRF_Enable(FALSE);
}
byte NRF_WriteRegister(int nCommand, int nData)
{
int nStatus;
//Every new command must be started by a high to low transition on CSN
NRF_Select(FALSE);
delay_us(1); //Tcwh = 50ns, CSN Inactive time
NRF_Select(TRUE); //Chip select
delay_us(1); //Tcsd = 38ns, CSN to Data Valid
nStatus = _spi_write(nCommand);
_spi_write(nData);
// NRF_Select(FALSE);
return nStatus;
}
uint8_t CX10::_spi_read_address(uint8_t address) {
uint8_t result;
CS_off;
_spi_write(address);
result = _spi_read();
CS_on;
return(result);
}
void NRF_config_rf()
{
//auto retransmit off
//NRF_WriteRegister(0x24, 0x00);
//Disable auto act
//NRF_WriteRegister(0x21, 0x00);
//auto retransmit 15 times, delay 500us
NRF_WriteRegister(0x24, 0x1F);
//Auto act on at all pipe
NRF_WriteRegister(0x21, 0x3F);
//Address width = 5
NRF_WriteRegister(0x23, 0x03);
//Data rate = 1MB
NRF_WriteRegister(0x26, 0x07);
//7 bytes Payload
NRF_WriteRegister(0x31, NRF_PACKET_SIZE);
//Set channel 2
NRF_WriteRegister(0x25, 0x02);
//Set TX address E7E7E7E7E7
NRF_WriteCommand(0x30);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
//Set RX address E7E7E7E7E7
NRF_WriteCommand(0x2A);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
_spi_write(0xE7);
NRF_Select(FALSE);
}
void CX10::Read_Packet() {
uint8_t i;
CS_off;
_spi_write(0x61); // Read RX payload
for (i=0;i<PACKET_LENGTH;i++) {
packet[i]=_spi_read();
}
CS_on;
}
// Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally
bool AP_Baro_MS5611::init()
{
//scheduler->suspend_timer();
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH);
delay(1);
_spi_write(CMD_MS5611_RESET);
delay(4);
// We read the factory calibration
// The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
C4 = _spi_read_16bits(CMD_MS5611_PROM_C4);
C5 = _spi_read_16bits(CMD_MS5611_PROM_C5);
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros();
_state = 0;
Temp=0;
Press=0;
_s_D1 = 0;
_s_D2 = 0;
_d1_count = 0;
_d2_count = 0;
//scheduler->resume_timer();
//scheduler->register_process( AP_Baro_MS5611::_update );
// wait for at least one value to be read
while (!_updated) ;
//healthy = true;
return true;
}
void NRF_WriteCommand(int nCommand)
{
//Every new command must be started by a high to low transition on CSN
NRF_Select(FALSE);
delay_us(1); //Tcwh = 50ns, CSN Inactive time
NRF_Select(TRUE); //Chip select
delay_us(1); //Tcsd = 38ns, CSN to Data Valid
_spi_write(nCommand);
}
byte NRF_ReadRegister(int nAddress)
{
byte nResult;
//Every new command must be started by a high to low transition on CSN
NRF_Select(FALSE);
delay_us(1); //Tcwh = 50ns, CSN Inactive time
NRF_Select(TRUE); //Chip select
delay_us(1); //Tcsd = 38ns, CSN to Data Valid
_spi_write(nAddress&0x1F);
nResult = _spi_read();
NRF_Select(FALSE);
return nResult;
}
static uint8_t optical_read(uint8_t addr) {
OPTICAL_CSEL_PORT &= ~(1<<OPTICAL_CSEL_BIT);
_spi_write(addr);
OPTICAL_SDIO_DDR &= ~(1<<OPTICAL_SDIO_BIT);
_delay_us(IO_DELAY);
uint8_t res = 0;
for (int8_t i=7; i>=0; i--) {
OPTICAL_SCLK_PORT &= ~(1<<OPTICAL_SCLK_BIT);
_delay_us(1);
OPTICAL_SCLK_PORT |= 1<<OPTICAL_SCLK_BIT;
_delay_us(1);
if (OPTICAL_SDIO_PIN & 1<<OPTICAL_SDIO_BIT) {
res |= 1<<i;
}
}
OPTICAL_CSEL_PORT |= (1<<OPTICAL_CSEL_BIT);
return res;
}
void spi_tx1_isr_handler(void)
{
struct spi_module *module = _spi_instances[1];
/* get interrupt flags and mask out enabled callbacks */
uint32_t flags = module->hw->TRANSMIT_STATUS.reg;
flags &= module->hw->TX_INTERRUPT_MASK.reg;
if (flags & SPI_TRANSMIT_STATUS_TX_FIFO_NOT_FULL_1) {
# if CONF_SPI_MASTER_ENABLE == true
if ((module->mode == SPI_MODE_MASTER) &&
(module->dir == SPI_DIRECTION_READ)) {
/* Send dummy byte when reading in master mode */
_spi_write_dummy(module);
if (module->remaining_dummy_buffer_length == 0) {
/* Disable the Data Register Empty Interrupt */
module->hw->TX_INTERRUPT_MASK.reg &=
~SPI_TX_INTERRUPT_MASK_TX_FIFO_NOT_FULL_MASK;
}
}
# endif
if (0
# if CONF_SPI_MASTER_ENABLE == true
|| ((module->mode == SPI_MODE_MASTER) &&
(module->dir != SPI_DIRECTION_READ))
# endif
# if CONF_SPI_SLAVE_ENABLE == true
|| ((module->mode == SPI_MODE_SLAVE) &&
(module->dir != SPI_DIRECTION_READ))
# endif
) {
_spi_write(module);
if (module->remaining_tx_buffer_length == 0) {
module->hw->TX_INTERRUPT_MASK.reg &=
~SPI_TX_INTERRUPT_MASK_TX_FIFO_NOT_FULL_MASK;
module->hw->TX_INTERRUPT_MASK.reg |=
SPI_TX_INTERRUPT_MASK_TX_FIFO_EMPTY_MASK;
}
}
}
if (flags & SPI_TRANSMIT_STATUS_TX_FIFO_EMPTY) {
if (module->dir == SPI_DIRECTION_WRITE) {
if ((module->enabled_callback & (1 << SPI_CALLBACK_BUFFER_TRANSMITTED)) &&
(module->registered_callback & (1 << SPI_CALLBACK_BUFFER_TRANSMITTED))) {
module->status = STATUS_OK;
/* Disable interrupt */
module->hw->TX_INTERRUPT_MASK.reg &=
~SPI_TX_INTERRUPT_MASK_TX_FIFO_EMPTY_MASK;
(module->callback[SPI_CALLBACK_BUFFER_TRANSMITTED])(module);
}
} else if (module->dir == SPI_DIRECTION_BOTH) {
if ((module->enabled_callback & (1 << SPI_CALLBACK_BUFFER_TRANSCEIVED)) &&
(module->registered_callback & (1 << SPI_CALLBACK_BUFFER_TRANSCEIVED))) {
/* Disable interrupt */
module->hw->TX_INTERRUPT_MASK.reg &=
~SPI_TX_INTERRUPT_MASK_TX_FIFO_EMPTY_MASK;
if (flag_direction_both[1]) {
module->status = STATUS_OK;
flag_direction_both[1] = false;
(module->callback[SPI_CALLBACK_BUFFER_TRANSCEIVED])(module);
} else {
flag_direction_both[1] = true;
}
}
}
}
}
//-------------------------------
//-------------------------------
//XN297 SPI routines
//-------------------------------
//-------------------------------
void CX10::Write_Packet(int slot, uint8_t init) { // 24 bytes total per packet (FIXME: wrong?)
Craft *c = &craft_[slot];
uint8_t i;
CS_off;
_spi_write(0xa0); // Write TX payload
_spi_write(init); // packet type: 0xaa or 0x55 aka bind packet or data packet)
_spi_write(c->txid[0]);
_spi_write(c->txid[1]);
_spi_write(c->txid[2]);
_spi_write(c->txid[3]);
_spi_write(c->aid[0]); // Aircraft ID
_spi_write(c->aid[1]);
_spi_write(c->aid[2]);
_spi_write(c->aid[3]);
// channels data
if (c->Servo_data[5] > 1500)
bitSet(c->Servo_data[3], 12);// Set flip mode based on chan6 input
cli(); // disable interrupts
packet[0]=lowByte(c->Servo_data[AILERON]);//low byte of servo timing(1000-2000us)
packet[1]=highByte(c->Servo_data[AILERON]);//high byte of servo timing(1000-2000us)
packet[2]=lowByte(c->Servo_data[ELEVATOR]);
packet[3]=highByte(c->Servo_data[ELEVATOR]);
packet[4]=lowByte(c->Servo_data[THROTTLE]);
packet[5]=highByte(c->Servo_data[THROTTLE]);
packet[6]=lowByte(c->Servo_data[RUDDER]);
packet[7]=highByte(c->Servo_data[RUDDER]);
sei(); // enable interrupts
for(i=0;i<4;i++){
_spi_write(packet[0+2*i]);
_spi_write(packet[1+2*i]);
}
// Set mode based on chan5 input
if (c->Servo_data[4] > 1800)
i = 0x02;// mode 3
else if (c->Servo_data[4] > 1300)
i = 0x01;// mode 2
else
i= 0x00;// mode 1
_spi_write(i);
_spi_write(0x00);
MOSI_off;
CS_on;
CE_on; // transmit
}
uint8_t _spi_read()
{
return _spi_write(0);
}
CX10::CX10() {
randomSeed((analogRead(A0) & 0x1F) | (analogRead(A1) << 5));
for (int n = 0; n < CRAFT; ++n) {
Craft *c = &craft_[n];
c->nextPacket = 0;
memset(c->aid, 0xff, sizeof c->aid);
memset(c->Servo_data, 0, sizeof c->Servo_data);
setThrottle(n, 0);
setAileron(n, 500);
setElevator(n, 500);
setRudder(n, 500);
for(uint8_t i=0;i<4;i++) {
#ifdef RFDUINO
c->txid[i] = random(256);
#else
c->txid[i] = random();
#endif
}
c->txid[1] %= 0x30;
c->freq[0] = (c->txid[0] & 0x0F) + 0x03;
c->freq[1] = (c->txid[0] >> 4) + 0x16;
c->freq[2] = (c->txid[1] & 0x0F) + 0x2D;
c->freq[3] = (c->txid[1] >> 4) + 0x40;
}
pinMode(ledPin, OUTPUT);
//RF module pins
pinMode(MOSI_pin, OUTPUT);
pinMode(SCK_pin, OUTPUT);
pinMode(MISO_pin, INPUT);
pinMode(CS_pin, OUTPUT);
pinMode(CE_pin, OUTPUT);
digitalWrite(ledPin, LOW);//start LED off
CS_on;//start CS high
CE_on;//start CE high
MOSI_on;//start MOSI high
SCK_on;//start sck high
delay(70);//wait 70ms
CS_off;//start CS low
CE_off;//start CE low
MOSI_off;//start MOSI low
SCK_off;//start sck low
delay(100);
CS_on;//start CS high
delay(10);
#ifdef RFDUINO
SPI.begin();
SPI.setFrequency(250);
#endif
//############ INIT1 ##############
CS_off;
_spi_write(0x3f); // Set Baseband parameters (debug registers) - BB_CAL
_spi_write(0x4c);
_spi_write(0x84);
_spi_write(0x67);
_spi_write(0x9c);
_spi_write(0x20);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x3e); // Set RF parameters (debug registers) - RF_CAL
_spi_write(0xc9);
_spi_write(0x9a);
_spi_write(0xb0);
_spi_write(0x61);
_spi_write(0xbb);
_spi_write(0xab);
_spi_write(0x9c);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x39); // Set Demodulator parameters (debug registers) - DEMOD_CAL
_spi_write(0x0b);
_spi_write(0xdf);
_spi_write(0xc4);
_spi_write(0xa7);
_spi_write(0x03);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x30); // Set TX address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
CS_off;
_spi_write(0x2a); // Set RX pipe 0 address 0xCCCCCCCC
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
_spi_write(0xcc);
CS_on;
delayMicroseconds(5);
_spi_write_address(0xe1, 0x00); // Clear TX buffer
_spi_write_address(0xe2, 0x00); // Clear RX buffer
_spi_write_address(0x27, 0x70); // Clear interrupts
_spi_write_address(0x21, 0x00); // No auto-acknowledge
_spi_write_address(0x22, 0x01); // Enable only data pipe 0
_spi_write_address(0x23, 0x03); // Set 5 byte rx/tx address field width
_spi_write_address(0x25, 0x02); // Set channel frequency
_spi_write_address(0x24, 0x00); // No auto-retransmit
_spi_write_address(0x31, PACKET_LENGTH); // 19-byte payload
_spi_write_address(0x26, 0x07); // 1 Mbps air data rate, 5dbm RF power
_spi_write_address(0x50, 0x73); // Activate extra feature register
_spi_write_address(0x3c, 0x00); // Disable dynamic payload length
_spi_write_address(0x3d, 0x00); // Extra features all off
MOSI_off;
delay(100);//100ms delay
//############ INIT2 ##############
healthy_ = _spi_read_address(0x10) == 0xCC;
_spi_write_address(0x20, 0x0e); // Power on, TX mode, 2 byte CRC
MOSI_off;
delay(100);
bindAllowed_ = true;
nextBind_ = micros();
nextSlot_ = 0;
}
