Browse Source

- add joystick readings

- port of cc2500 class from multi protocol sender
master
philipp schoenberger 7 years ago
parent
commit
69dd205971
  1. 169
      remote/src/cc2500.cpp
  2. 84
      remote/src/cc2500.h
  3. 4
      remote/src/config.h
  4. 150
      remote/src/iface_cc2500.h
  5. 103
      remote/src/io_data.cpp
  6. 24
      remote/src/io_data.h
  7. 69
      remote/src/main.cpp
  8. 250
      remote/src/pins.h
  9. 75
      remote/src/spi.cpp
  10. 28
      remote/src/spi.h

169
remote/src/cc2500.cpp

@ -0,0 +1,169 @@
#include "cc2500.h"
#include "config.h"
#include "pins.h"
#include "Arduino.h"
CC2500::CC2500(SPI* spi)
{
this->spi = spi;
this->bind_done = false;
}
void CC2500::write_register(uint8_t address, uint8_t data)
{
// select chip
CC2500_CS_OFF;
// first address then data
spi->write(address);
NOP;
spi->write(data);
// deselect chip
CC2500_CS_ON;
}
void CC2500::write_register_multi(uint8_t address, const uint8_t data[], uint8_t length)
{
CC2500_CS_OFF;
spi->write(CC2500_WRITE_BURST | address);
for(uint8_t i = 0; i < length; i++) {
spi->write(data[i]);
}
CC2500_CS_ON;
}
void CC2500::write_data(uint8_t *dpbuffer, uint8_t len)
{
this->strobe(CC2500_SFTX);
this->write_register_multi(CC2500_3F_TXFIFO, dpbuffer, len);
this->strobe(CC2500_STX);
}
void CC2500::read_register_multi(uint8_t address, uint8_t data[], uint8_t length)
{
// select chip
CC2500_CS_OFF;
spi->write(CC2500_READ_BURST | address);
for(uint8_t i = 0; i < length; i++) {
data[i] = spi->read();
}
// deselect chip
CC2500_CS_ON;
}
uint8_t CC2500::read_register(uint8_t address)
{
uint8_t result;
// select chip
CC2500_CS_OFF;
spi->write(CC2500_READ_SINGLE | address);
result = spi->read();
// deselect chip
CC2500_CS_ON;
return(result);
}
void CC2500::read_data(uint8_t *dpbuffer, uint8_t len)
{
this->read_register_multi(CC2500_3F_RXFIFO, dpbuffer, len);
}
void CC2500::strobe(uint8_t state)
{
CC2500_CS_OFF;
spi->write(state);
CC2500_CS_ON;
}
void CC2500::set_wireless_mode(enum wireless_mode mode )
{
switch (mode) {
case wireless_tx:
this->write_register(CC2500_00_IOCFG2, 0x2F);
this->write_register(CC2500_02_IOCFG0, 0x2F | 0x40);
break;
case wireless_rx:
this->write_register(CC2500_02_IOCFG0, 0x2F);
this->write_register(CC2500_00_IOCFG2, 0x2F | 0x40);
break;
case wireless_off:
this->write_register(CC2500_02_IOCFG0, 0x2F);
this->write_register(CC2500_00_IOCFG2, 0x2F);
break;
}
}
void CC2500::set_power(void)
{
uint8_t power = CC2500_BIND_POWER;
if(this->bind_done) {
#ifdef CC2500_ENABLE_LOW_POWER
power=IS_POWER_FLAG_on?CC2500_HIGH_POWER:CC2500_LOW_POWER;
#else
power=CC2500_HIGH_POWER;
#endif
}
if(IS_RANGE_FLAG_on) {
power=CC2500_RANGE_POWER;
}
if(this->prev_power != power) {
this->write_register(CC2500_3E_PATABLE, power);
this->prev_power = power;
}
}
uint8_t CC2500::reset()
{
this->strobe(CC2500_SRES);
delay(1); //ms
this->set_wireless_mode(wireless_off);
return this->read_register(CC2500_0E_FREQ1) == 0xC4;//check if reset
}
void CC2500::resetChip(void)
{
// Toggle chip select signal
CC2500_CS_ON;
delayMicroseconds(30);
CC2500_CS_OFF;
delayMicroseconds(30);
CC2500_CS_ON;
delayMicroseconds(45);
this->strobe(CC2500_SRES);
_delay_ms(100);
}
void CC2500::set_power_value(uint8_t power)
{
const unsigned char patable[8]= {
0xC5, // -12dbm
0x97, // -10dbm
0x6E, // -8dbm
0x7F, // -6dbm
0xA9, // -4dbm
0xBB, // -2dbm
0xFE, // 0dbm
0xFF // 1.5dbm
};
if (power > 7)
power = 7;
this->write_register(CC2500_3E_PATABLE, patable[power]);
}

84
remote/src/cc2500.h

@ -0,0 +1,84 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef CC2500_H
#include "Arduino.h"
#include "spi.h"
#include "iface_cc2500.h"
class CC2500 {
private:
SPI* spi;
bool bind_done;
uint8_t prev_power;
public:
//TODO IS_RANGE_FLAG_on
#define IS_RANGE_FLAG_on false
enum wireless_mode {
wireless_off,
wireless_tx,
wireless_rx,
};
// CC2500 power output from the chip itself
// The numbers do not take into account any outside amplifier
enum CC2500_POWER
{
CC2500_POWER_0 = 0x00, // -55dbm or less
CC2500_POWER_1 = 0x50, // -30dbm
CC2500_POWER_2 = 0x44, // -28dbm
CC2500_POWER_3 = 0xC0, // -26dbm
CC2500_POWER_4 = 0x84, // -24dbm
CC2500_POWER_5 = 0x81, // -22dbm
CC2500_POWER_6 = 0x46, // -20dbm
CC2500_POWER_7 = 0x93, // -18dbm
CC2500_POWER_8 = 0x55, // -16dbm
CC2500_POWER_9 = 0x8D, // -14dbm
CC2500_POWER_10 = 0xC6, // -12dbm
CC2500_POWER_11 = 0x97, // -10dbm
CC2500_POWER_12 = 0x6E, // -8dbm
CC2500_POWER_13 = 0x7F, // -6dbm
CC2500_POWER_14 = 0xA9, // -4dbm
CC2500_POWER_15 = 0xBB, // -2dbm
CC2500_POWER_16 = 0xFE, // 0dbm
CC2500_POWER_17 = 0xFF, // +1dbm
CC2500_HIGH_POWER = CC2500_POWER_17,
CC2500_LOW_POWER = CC2500_POWER_13,
CC2500_RANGE_POWER = CC2500_POWER_1,
CC2500_BIND_POWER = CC2500_POWER_1,
};
CC2500(SPI *spi);
void write_register(uint8_t address, uint8_t data);
void write_register_multi(uint8_t address, const uint8_t data[], uint8_t length);
void write_data(uint8_t *dpbuffer, uint8_t len);
void read_register_multi(uint8_t address, uint8_t data[], uint8_t length);
uint8_t read_register(uint8_t address);
void read_data(uint8_t *dpbuffer, uint8_t len);
void strobe(uint8_t state);
void set_wireless_mode(enum wireless_mode mode );
void set_power(void);
uint8_t reset(void);
void set_power_value(uint8_t power);
void resetChip(void);
};
#endif

4
remote/src/config.h

@ -0,0 +1,4 @@
#ifndef CONFIG_H
#define CONFIG_H
#endif

150
remote/src/iface_cc2500.h

@ -0,0 +1,150 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _IFACE_CC2500_H_
#define _IFACE_CC2500_H_
enum {
CC2500_00_IOCFG2 = 0x00, // GDO2 output pin configuration
CC2500_01_IOCFG1 = 0x01, // GDO1 output pin configuration
CC2500_02_IOCFG0 = 0x02, // GDO0 output pin configuration
CC2500_03_FIFOTHR = 0x03, // RX FIFO and TX FIFO thresholds
CC2500_04_SYNC1 = 0x04, // Sync word, high byte
CC2500_05_SYNC0 = 0x05, // Sync word, low byte
CC2500_06_PKTLEN = 0x06, // Packet length
CC2500_07_PKTCTRL1 = 0x07, // Packet automation control
CC2500_08_PKTCTRL0 = 0x08, // Packet automation control
CC2500_09_ADDR = 0x09, // Device address
CC2500_0A_CHANNR = 0x0A, // Channel number
CC2500_0B_FSCTRL1 = 0x0B, // Frequency synthesizer control
CC2500_0C_FSCTRL0 = 0x0C, // Frequency synthesizer control
CC2500_0D_FREQ2 = 0x0D, // Frequency control word, high byte
CC2500_0E_FREQ1 = 0x0E, // Frequency control word, middle byte
CC2500_0F_FREQ0 = 0x0F, // Frequency control word, low byte
CC2500_10_MDMCFG4 = 0x10, // Modem configuration
CC2500_11_MDMCFG3 = 0x11, // Modem configuration
CC2500_12_MDMCFG2 = 0x12, // Modem configuration
CC2500_13_MDMCFG1 = 0x13, // Modem configuration
CC2500_14_MDMCFG0 = 0x14, // Modem configuration
CC2500_15_DEVIATN = 0x15, // Modem deviation setting
CC2500_16_MCSM2 = 0x16, // Main Radio Cntrl State Machine config
CC2500_17_MCSM1 = 0x17, // Main Radio Cntrl State Machine config
CC2500_18_MCSM0 = 0x18, // Main Radio Cntrl State Machine config
CC2500_19_FOCCFG = 0x19, // Frequency Offset Compensation config
CC2500_1A_BSCFG = 0x1A, // Bit Synchronization configuration
CC2500_1B_AGCCTRL2 = 0x1B, // AGC control
CC2500_1C_AGCCTRL1 = 0x1C, // AGC control
CC2500_1D_AGCCTRL0 = 0x1D, // AGC control
CC2500_1E_WOREVT1 = 0x1E, // High byte Event 0 timeout
CC2500_1F_WOREVT0 = 0x1F, // Low byte Event 0 timeout
CC2500_20_WORCTRL = 0x20, // Wake On Radio control
CC2500_21_FREND1 = 0x21, // Front end RX configuration
CC2500_22_FREND0 = 0x22, // Front end TX configuration
CC2500_23_FSCAL3 = 0x23, // Frequency synthesizer calibration
CC2500_24_FSCAL2 = 0x24, // Frequency synthesizer calibration
CC2500_25_FSCAL1 = 0x25, // Frequency synthesizer calibration
CC2500_26_FSCAL0 = 0x26, // Frequency synthesizer calibration
CC2500_27_RCCTRL1 = 0x27, // RC oscillator configuration
CC2500_28_RCCTRL0 = 0x28, // RC oscillator configuration
CC2500_29_FSTEST = 0x29, // Frequency synthesizer cal control
CC2500_2A_PTEST = 0x2A, // Production test
CC2500_2B_AGCTEST = 0x2B, // AGC test
CC2500_2C_TEST2 = 0x2C, // Various test settings
CC2500_2D_TEST1 = 0x2D, // Various test settings
CC2500_2E_TEST0 = 0x2E, // Various test settings
// Status registers
CC2500_30_PARTNUM = 0x30, // Part number
CC2500_31_VERSION = 0x31, // Current version number
CC2500_32_FREQEST = 0x32, // Frequency offset estimate
CC2500_33_LQI = 0x33, // Demodulator estimate for link quality
CC2500_34_RSSI = 0x34, // Received signal strength indication
CC2500_35_MARCSTATE = 0x35, // Control state machine state
CC2500_36_WORTIME1 = 0x36, // High byte of WOR timer
CC2500_37_WORTIME0 = 0x37, // Low byte of WOR timer
CC2500_38_PKTSTATUS = 0x38, // Current GDOx status and packet status
CC2500_39_VCO_VC_DAC = 0x39, // Current setting from PLL cal module
CC2500_3A_TXBYTES = 0x3A, // Underflow and # of bytes in TXFIFO
CC2500_3B_RXBYTES = 0x3B, // Overflow and # of bytes in RXFIFO
// Multi byte memory locations
CC2500_3E_PATABLE = 0x3E,
CC2500_3F_TXFIFO = 0x3F,
CC2500_3F_RXFIFO = 0x3F,
};
// Definitions for burst/single access to registers
#define CC2500_WRITE_SINGLE 0x00
#define CC2500_WRITE_BURST 0x40
#define CC2500_READ_SINGLE 0x80
#define CC2500_READ_BURST 0xC0
// Strobe commands
#define CC2500_SRES 0x30 // Reset chip.
#define CC2500_SFSTXON 0x31 // Enable and calibrate frequency synthesizer (if MCSM0.FS_AUTOCAL=1).
// If in RX/TX: Go to a wait state where only the synthesizer is
// running (for quick RX / TX turnaround).
#define CC2500_SXOFF 0x32 // Turn off crystal oscillator.
#define CC2500_SCAL 0x33 // Calibrate frequency synthesizer and turn it off
// (enables quick start).
#define CC2500_SRX 0x34 // Enable RX. Perform calibration first if coming from IDLE and
// MCSM0.FS_AUTOCAL=1.
#define CC2500_STX 0x35 // In IDLE state: Enable TX. Perform calibration first if
// MCSM0.FS_AUTOCAL=1. If in RX state and CCA is enabled:
// Only go to TX if channel is clear.
#define CC2500_SIDLE 0x36 // Exit RX / TX, turn off frequency synthesizer and exit
// Wake-On-Radio mode if applicable.
#define CC2500_SAFC 0x37 // Perform AFC adjustment of the frequency synthesizer
#define CC2500_SWOR 0x38 // Start automatic RX polling sequence (Wake-on-Radio)
#define CC2500_SPWD 0x39 // Enter power down mode when CSn goes high.
#define CC2500_SFRX 0x3A // Flush the RX FIFO buffer.
#define CC2500_SFTX 0x3B // Flush the TX FIFO buffer.
#define CC2500_SWORRST 0x3C // Reset real time clock.
#define CC2500_SNOP 0x3D // No operation. May be used to pad strobe commands to two
// bytes for simpler software.
//----------------------------------------------------------------------------------
// Chip Status Byte
//----------------------------------------------------------------------------------
// Bit fields in the chip status byte
#define CC2500_STATUS_CHIP_RDYn_BM 0x80
#define CC2500_STATUS_STATE_BM 0x70
#define CC2500_STATUS_FIFO_BYTES_AVAILABLE_BM 0x0F
// Chip states
#define CC2500_STATE_IDLE 0x00
#define CC2500_STATE_RX 0x10
#define CC2500_STATE_TX 0x20
#define CC2500_STATE_FSTXON 0x30
#define CC2500_STATE_CALIBRATE 0x40
#define CC2500_STATE_SETTLING 0x50
#define CC2500_STATE_RX_OVERFLOW 0x60
#define CC2500_STATE_TX_UNDERFLOW 0x70
//----------------------------------------------------------------------------------
// Other register bit fields
//----------------------------------------------------------------------------------
#define CC2500_LQI_CRC_OK_BM 0x80
#define CC2500_LQI_EST_BM 0x7F
//void CC2500_WriteReg(u8 addr, u8 data);
//u8 CC2500_ReadReg(u8 addr);
//void CC2500_Reset();
//void CC2500_Strobe(u8 cmd);
//void CC2500_WriteData(u8 *packet, u8 length);
//void CC2500_ReadData(u8 *dpbuffer, int len);
//void CC2500_SetTxRxMode(enum TXRX_State);
#endif

103
remote/src/io_data.cpp

@ -0,0 +1,103 @@
#include "io_data.h"
#include "pins.h"
#include <Arduino.h>
IO_DATA::IO_DATA()
{
}
void IO_DATA::adc_init(void)
{
// AREF = AVcc
ADMUX = (0<<REFS1) | (1<<REFS0);
Serial.print("ADMUX");
Serial.print(ADMUX);
Serial.print(" ADCSRA");
Serial.print(ADCSRA);
Serial.print("\n");
// ADC Enable
ADCSRA |= _BV(ADEN);
// ADC prescaler of 128
// 16000000/128 = 125000
ADCSRA |= _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);
}
uint16_t IO_DATA::adc_read(uint8_t ch)
{
// ‘ch’ between 0 and 7
//
if ( ch >= 0x08 ) {
/* Set Mux5 */
ADCSRB |= _BV(MUX5);
ch &= ~0x08;
} else {
/* Clear Mux5 */
ADCSRB &= ~_BV(MUX5);
}
ch = ch & 0b00000111;
Serial.print("ch");
Serial.print(ch);
ADMUX |= ch;
// start single convertion
ADCSRA |= (1<<ADSC);
// wait for conversion to complete
delay(50);
while (ADCSRA & (1<<ADSC)); // wait for conversion to complete
/* while(!(ADCSRA & (1<<ADIF))) { */
/* /1* nothing *1/ */
/* } */
// clear bit
ADCSRA |= (1<<ADIF);
return (ADC);
}
void IO_DATA::init()
{
#if 0
adc_init();
#endif
BUTTON_R_INPUT;
BUTTON_L_INPUT;
ARM_INPUT;
FLIGHT_MODE_INPUT;
/* inital read */
update();
}
void IO_DATA::update()
{
#if 0
/* read joysticks */
r_x = adc_read(AD_R_X_JOY_PIN);
r_y = adc_read(AD_R_Y_JOY_PIN);
l_x = adc_read(AD_L_X_JOY_PIN);
l_y = adc_read(AD_L_Y_JOY_PIN);
/* read akku power */
akku_power = adc_read(AD_AKKUPOWER_PIN);
#else
/* read joysticks */
r_x = analogRead(AD_R_X_JOY_PIN);
r_y = analogRead(AD_R_Y_JOY_PIN);
l_x = analogRead(AD_L_X_JOY_PIN);
l_y = analogRead(AD_L_Y_JOY_PIN);
/* read akku power */
akku_power = analogRead(AD_AKKUPOWER_PIN);
#endif
r_sw = BUTTON_R_IS_HIGH;
l_sw = BUTTON_L_IS_HIGH;
/* read switches */
arm_sw = ARM_IS_HIGH;
flight_mode_sw = FLIGHT_MODE_IS_HIGH;
}

24
remote/src/io_data.h

@ -0,0 +1,24 @@
#ifndef INPUT_HPP
#define INPUT_HPP
#include "Arduino.h"
class IO_DATA {
private:
void adc_init(void);
uint16_t adc_read(uint8_t ch);
public:
uint16_t r_x, r_y;
bool r_sw;
uint16_t l_x, l_y;
bool l_sw;
bool arm_sw;
bool flight_mode_sw;
uint16_t akku_power;
IO_DATA();
void init();
void update();
};
#endif /* INPUT_HPP */

69
remote/src/main.cpp

@ -1,19 +1,72 @@
//#include <SPI.h>
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#include <Arduino.h>
#include "spi.h"
#include "cc2500.h"
#include "io_data.h"
SPI* spi = NULL;
IO_DATA* io_data = NULL;
CC2500* cc2500 = NULL;
void setup() {
/* while(Serial) { */
/* delay(500); */
/* } */
while(!Serial) {
delay(500);
}
Serial.begin(115200);
Serial.println("Remote ON!");
//SPI.begin();
//CC2500.init();
spi = new SPI();
spi->init();
cc2500 = new CC2500(spi);
io_data = new IO_DATA();
io_data->init();
Serial.println("Remote ON!");
}
void loop() {
// put your main code here, to run repeatedly:
Serial.println("loop!");
io_data->update();
Serial.print(" akku= ");
Serial.print(io_data->akku_power);
Serial.print(" arm= ");
Serial.print(io_data->arm_sw);
Serial.print(" mode= ");
Serial.print(io_data->flight_mode_sw);
Serial.print("l x= ");
Serial.print(io_data->r_x);
Serial.print(" y= ");
Serial.print(io_data->r_y);
Serial.print(" sw= ");
Serial.print(io_data->r_sw);
Serial.print("r x= ");
Serial.print(io_data->l_x);
Serial.print(" y= ");
Serial.print(io_data->l_y);
Serial.print(" sw= ");
Serial.print(io_data->l_sw);
Serial.print("\n");
delay(500);
}

250
remote/src/pins.h

@ -0,0 +1,250 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* LEONARDO usb
* __Micro__
* D1 -| |- RAW
* D0 -| |- GND
* GND -| |- RESET
* GND -| |- VCC
* D2 -| |- A3
* D3 -| |- A2
* D4/A6 -| |- A1
* D5 -| |- A0
* D6/A7 -| |- D15
* D7 -| |- D14
* D8/A8 -| |- D16
* D9/A9 -| |- D10/A10
* '''''''''
*/
//*******************
//*** Pinouts ***
//*******************
/* i2c clock */
#define I2C_SLC_PIN 3
#define I2C_SLC_PORT PORTD
#define I2C_SLC_IPR PIND
#define I2C_SLC_DDR DDRD
#define I2C_SLC_ON I2C_SLC_PORT |= _BV(I2C_SLC_PIN)
#define I2C_SLC_OFF I2C_SLC_PORT &= ~_BV(I2C_SLC_PIN)
#define I2C_SLC_OUTPUT I2C_SLC_DDR |= _BV(I2C_SLC_PIN)
#define I2C_SLC_INPUT I2C_SLC_DDR &= ~_BV(I2C_SLC_PIN)
#define I2C_SLC_IS_HIGH ((I2C_SLC_IPR & _BV(I2C_SLC_PIN)) != 0x00)
#define I2C_SLC_IS_LOW ((I2C_SLC_IPR & _BV(I2C_SLC_PIN)) == 0x00)
/* i2c data */
#define I2C_SDA_PIN 2
#define I2C_SDA_PORT PORTD
#define I2C_SDA_IPR PIND
#define I2C_SDA_DDR DDRD
#define I2C_SDA_ON I2C_SDA_PORT |= _BV(I2C_SDA_PIN)
#define I2C_SDA_OFF I2C_SDA_PORT &= ~_BV(I2C_SDA_PIN)
#define I2C_SDA_OUTPUT I2C_SDA_DDR |= _BV(I2C_SDA_PIN)
#define I2C_SDA_INPUT I2C_SDA_DDR &= ~_BV(I2C_SDA_PIN)
#define I2C_SDA_IS_HIGH ((I2C_SDA_IPR & _BV(I2C_SDA_PIN)) != 0x00)
#define I2C_SDA_IS_LOW ((I2C_SDA_IPR & _BV(I2C_SDA_PIN)) == 0x00)
/* switch flight mode */
#define FLIGHT_MODE_PIN 0
#define FLIGHT_MODE_PORT PORTD
#define FLIGHT_MODE_IPR PIND
#define FLIGHT_MODE_DDR DDRD
#define FLIGHT_MODE_ON FLIGHT_MODE_PORT |= _BV(FLIGHT_MODE_PIN)
#define FLIGHT_MODE_OFF FLIGHT_MODE_PORT &= ~_BV(FLIGHT_MODE_PIN)
#define FLIGHT_MODE_OUTPUT FLIGHT_MODE_DDR |= _BV(FLIGHT_MODE_PIN)
#define FLIGHT_MODE_INPUT FLIGHT_MODE_DDR &= ~_BV(FLIGHT_MODE_PIN)
#define FLIGHT_MODE_IS_HIGH ((FLIGHT_MODE_IPR & _BV(FLIGHT_MODE_PIN)) != 0x00)
#define FLIGHT_MODE_IS_LOW ((FLIGHT_MODE_IPR & _BV(FLIGHT_MODE_PIN)) == 0x00)
/* switch arm */
#define ARM_PIN 1
#define ARM_PORT PORTD
#define ARM_IPR PIND
#define ARM_DDR DDRD
#define ARM_ON ARM_PORT |= _BV(ARM_PIN)
#define ARM_OFF ARM_PORT &= ~_BV(ARM_PIN)
#define ARM_OUTPUT ARM_DDR |= _BV(ARM_PIN)
#define ARM_INPUT ARM_DDR &= ~_BV(ARM_PIN)
#define ARM_IS_HIGH ((ARM_IPR & _BV(ARM_PIN)) != 0x00)
#define ARM_IS_LOW ((ARM_IPR & _BV(ARM_PIN)) == 0x00)
/* power amplifier */
#define POWER_AMP_PIN 5
#define POWER_AMP_PORT PORTD
#define POWER_AMP_IPR PIND
#define POWER_AMP_DDR DDRD
#define POWER_AMP_ON POWER_AMP_PORT |= _BV(POWER_AMP_PIN)
#define POWER_AMP_OFF POWER_AMP_PORT &= ~_BV(POWER_AMP_PIN)
#define POWER_AMP_OUTPUT POWER_AMP_DDR |= _BV(POWER_AMP_PIN)
#define POWER_AMP_INPUT POWER_AMP_DDR &= ~_BV(POWER_AMP_PIN)
#define POWER_AMP_IS_HIGH ((POWER_AMP_IPR & _BV(POWER_AMP_PIN)) != 0x00)
#define POWER_AMP_IS_LOW ((POWER_AMP_IPR & _BV(POWER_AMP_PIN)) == 0x00)
/* low noise amplifier */
#define LOW_NOISE_AMP_PIN 5
#define LOW_NOISE_AMP_PORT PORTD
#define LOW_NOISE_AMP_IPR PIND
#define LOW_NOISE_AMP_DDR DDRD
#define LOW_NOISE_AMP_ON LOW_NOISE_AMP_PORT |= _BV(LOW_NOISE_AMP_PIN)
#define LOW_NOISE_AMP_OFF LOW_NOISE_AMP_PORT &= ~_BV(LOW_NOISE_AMP_PIN)
#define LOW_NOISE_AMP_OUTPUT LOW_NOISE_AMP_DDR |= _BV(LOW_NOISE_AMP_PIN)
#define LOW_NOISE_AMP_INPUT LOW_NOISE_AMP_DDR &= ~_BV(LOW_NOISE_AMP_PIN)
#define LOW_NOISE_AMP_IS_HIGH ((LOW_NOISE_AMP_IPR & _BV(LOW_NOISE_AMP_PIN)) != 0x00)
#define LOW_NOISE_AMP_IS_LOW ((LOW_NOISE_AMP_IPR & _BV(LOW_NOISE_AMP_PIN)) == 0x00)
/* buzzer */
#define BUZZER_PIN 8
#define BUZZER_PORT PORTD
#define BUZZER_IPR PIND
#define BUZZER_DDR DDRD
#define BUZZER_ON BUZZER_PORT |= _BV(BUZZER_PIN)
#define BUZZER_OFF BUZZER_PORT &= ~_BV(BUZZER_PIN)
#define BUZZER_OUTPUT BUZZER_DDR |= _BV(BUZZER_PIN)
#define BUZZER_INPUT BUZZER_DDR &= ~_BV(BUZZER_PIN)
#define BUZZER_IS_HIGH ((BUZZER_IPR & _BV(BUZZER_PIN)) != 0x00)
#define BUZZER_IS_LOW ((BUZZER_IPR & _BV(BUZZER_PIN)) == 0x00)
/* chip select cc2500 */
#define CC2500_CS_PIN 8
#define CC2500_CS_PORT PORTD
#define CC2500_CS_IPR PIND
#define CC2500_CS_DDR DDRD
#define CC2500_CS_ON CC2500_CS_PORT |= _BV(CC2500_CS_PIN)
#define CC2500_CS_OFF CC2500_CS_PORT &= ~_BV(CC2500_CS_PIN)
#define CC2500_CS_OUTPUT CC2500_CS_DDR |= _BV(CC2500_CS_PIN)
#define CC2500_CS_INPUT CC2500_CS_DDR &= ~_BV(CC2500_CS_PIN)
#define CC2500_CS_IS_HIGH ((CC2500_CS_IPR & _BV(CC2500_CS_PIN)) != 0x00)
#define CC2500_CS_IS_LOW ((CC2500_CS_IPR & _BV(CC2500_CS_PIN)) == 0x00)
/* Analog right Joystick X direction */
#define AD_R_X_JOY_PIN 3
#define AD_R_X_JOY_PORT PORTA
#define AD_R_X_JOY_IPR PORTA
#define AD_R_X_JOY_DDR DDRA
#define AD_R_X_JOY_ON AD_R_X_JOY_PORT |= _BV(AD_R_X_JOY_PIN)
#define AD_R_X_JOY_OFF AD_R_X_JOY_PORT &= ~_BV(AD_R_X_JOY_PIN)
#define AD_R_X_JOY_OUTPUT AD_R_X_JOY_DDR |= _BV(AD_R_X_JOY_PIN)
#define AD_R_X_JOY_INPUT AD_R_X_JOY_DDR &= ~_BV(AD_R_X_JOY_PIN)
#define AD_R_X_JOY_IS_HIGH ((AD_R_X_JOY_IPR & _BV(AD_R_X_JOY_PIN)) != 0x00)
#define AD_R_X_JOY_IS_LOW ((AD_R_X_JOY_IPR & _BV(AD_R_X_JOY_PIN)) == 0x00)
/* Analog right Joystick y direction */
#define AD_R_Y_JOY_PIN 2
#define AD_R_Y_JOY_PORT PORTA
#define AD_R_Y_JOY_IPR PORTA
#define AD_R_Y_JOY_DDR DDRA
#define AD_R_Y_JOY_ON AD_R_Y_JOY_PORT |= _BV(AD_R_Y_JOY_PIN)
#define AD_R_Y_JOY_OFF AD_R_Y_JOY_PORT &= ~_BV(AD_R_Y_JOY_PIN)
#define AD_R_Y_JOY_OUTPUT AD_R_Y_JOY_DDR |= _BV(AD_R_Y_JOY_PIN)
#define AD_R_Y_JOY_INPUT AD_R_Y_JOY_DDR &= ~_BV(AD_R_Y_JOY_PIN)
#define AD_R_Y_JOY_IS_HIGH ((AD_R_Y_JOY_IPR & _BV(AD_R_Y_JOY_PIN)) != 0x00)
#define AD_R_Y_JOY_IS_LOW ((AD_R_Y_JOY_IPR & _BV(AD_R_Y_JOY_PIN)) == 0x00)
/* right joystick middle button */
#define BUTTON_R_PIN 8
#define BUTTON_R_PORT PORTD
#define BUTTON_R_IPR PIND
#define BUTTON_R_DDR DDRD
#define BUTTON_R_ON BUTTON_R_PORT |= _BV(BUTTON_R_PIN)
#define BUTTON_R_OFF BUTTON_R_PORT &= ~_BV(BUTTON_R_PIN)
#define BUTTON_R_OUTPUT BUTTON_R_DDR |= _BV(BUTTON_R_PIN)
#define BUTTON_R_INPUT BUTTON_R_DDR &= ~_BV(BUTTON_R_PIN)
#define BUTTON_R_IS_HIGH ((BUTTON_R_IPR & _BV(BUTTON_R_PIN)) != 0x00)
#define BUTTON_R_IS_LOW ((BUTTON_R_IPR & _BV(BUTTON_R_PIN)) == 0x00)
/* Analog left Joystick X direction */
#define AD_L_X_JOY_PIN 1
#define AD_L_X_JOY_PORT PORTA
#define AD_L_X_JOY_IPR PORTA
#define AD_L_X_JOY_DDR DDRA
#define AD_L_X_JOY_ON AD_L_X_JOY_PORT |= _BV(AD_L_X_JOY_PIN)
#define AD_L_X_JOY_OFF AD_L_X_JOY_PORT &= ~_BV(AD_L_X_JOY_PIN)
#define AD_L_X_JOY_OUTPUT AD_L_X_JOY_DDR |= _BV(AD_L_X_JOY_PIN)
#define AD_L_X_JOY_INPUT AD_L_X_JOY_DDR &= ~_BV(AD_L_X_JOY_PIN)
#define AD_L_X_JOY_IS_HIGH ((AD_L_X_JOY_IPR & _BV(AD_L_X_JOY_PIN)) != 0x00)
#define AD_L_X_JOY_IS_LOW ((AD_L_X_JOY_IPR & _BV(AD_L_X_JOY_PIN)) == 0x00)
/* Analog left Joystick y direction */
#define AD_L_Y_JOY_PIN 0
#define AD_L_Y_JOY_PORT PORTA
#define AD_L_Y_JOY_IPR PORTA
#define AD_L_Y_JOY_DDR DDRA
#define AD_L_Y_JOY_ON AD_L_Y_JOY_PORT |= _BV(AD_L_Y_JOY_PIN)
#define AD_L_Y_JOY_OFF AD_L_Y_JOY_PORT &= ~_BV(AD_L_Y_JOY_PIN)
#define AD_L_Y_JOY_OUTPUT AD_L_Y_JOY_DDR |= _BV(AD_L_Y_JOY_PIN)
#define AD_L_Y_JOY_INPUT AD_L_Y_JOY_DDR &= ~_BV(AD_L_Y_JOY_PIN)
#define AD_L_Y_JOY_IS_HIGH ((AD_L_Y_JOY_IPR & _BV(AD_L_Y_JOY_PIN)) != 0x00)
#define AD_L_Y_JOY_IS_LOW ((AD_L_Y_JOY_IPR & _BV(AD_L_Y_JOY_PIN)) == 0x00)
/* left joystick middle button */
#define BUTTON_L_PIN 8
#define BUTTON_L_PORT PORTD
#define BUTTON_L_IPR PIND
#define BUTTON_L_DDR DDRD
#define BUTTON_L_ON BUTTON_L_PORT |= _BV(BUTTON_L_PIN)
#define BUTTON_L_OFF BUTTON_L_PORT &= ~_BV(BUTTON_L_PIN)
#define BUTTON_L_OUTPUT BUTTON_L_DDR |= _BV(BUTTON_L_PIN)
#define BUTTON_L_INPUT BUTTON_L_DDR &= ~_BV(BUTTON_L_PIN)
#define BUTTON_L_IS_HIGH ((BUTTON_L_IPR & _BV(BUTTON_L_PIN)) != 0x00)
#define BUTTON_L_IS_LOW ((BUTTON_L_IPR & _BV(BUTTON_L_PIN)) == 0x00)
/* Analog akku power read */
#define AD_AKKUPOWER_PIN 10
#define AD_AKKUPOWER_PORT PORTA
#define AD_AKKUPOWER_IPR PORTA
#define AD_AKKUPOWER_DDR DDRA
#define AD_AKKUPOWER_ON AD_AKKUPOWER_PORT |= _BV(AD_AKKUPOWER_PIN)
#define AD_AKKUPOWER_OFF AD_AKKUPOWER_PORT &= ~_BV(AD_AKKUPOWER_PIN)
#define AD_AKKUPOWER_OUTPUT AD_AKKUPOWER_PORT |= _BV(AD_AKKUPOWER_PIN)
#define AD_AKKUPOWER_INPUT AD_AKKUPOWER_PORT &= ~_BV(AD_AKKUPOWER_PIN)
#define AD_AKKUPOWER_IS_HIGH ((AD_AKKUPOWER_IPR & _BV(AD_AKKUPOWER_PIN)) != 0x00)
#define AD_AKKUPOWER_IS_LOW ((AD_AKKUPOWER_IPR & _BV(AD_AKKUPOWER_PIN)) == 0x00)
/* SPI clock */
#define SPI_SCLK_PIN 15
#define SPI_SCLK_PORT PORTD
#define SPI_SCLK_IPR PIND
#define SPI_SCLK_DDR DDRD
#define SPI_SCLK_ON SPI_SCLK_PORT |= _BV(SPI_SCLK_PIN)
#define SPI_SCLK_OFF SPI_SCLK_PORT &= ~_BV(SPI_SCLK_PIN)
#define SPI_SCLK_OUTPUT SPI_SCLK_DDR |= _BV(SPI_SCLK_PIN)
#define SPI_SCLK_INPUT SPI_SCLK_DDR &= ~_BV(SPI_SCLK_PIN)
#define SPI_SCLK_IS_HIGH ((SPI_SCLK_IPR & _BV(SPI_SCLK_PIN)) != 0x00)
#define SPI_SCLK_IS_LOW ((SPI_SCLK_IPR & _BV(SPI_SCLK_PIN)) == 0x00)
/* SPI master in slave out */
#define SPI_MISO_PIN 14
#define SPI_MISO_PORT PORTD
#define SPI_MISO_IPR PIND
#define SPI_MISO_DDR DDRD
#define SPI_MISO_ON SPI_MISO_PORT |= _BV(SPI_MISO_PIN)
#define SPI_MISO_OFF SPI_MISO_PORT &= ~_BV(SPI_MISO_PIN)
#define SPI_MISO_OUTPUT SPI_MISO_DDR |= _BV(SPI_MISO_PIN)
#define SPI_MISO_INPUT SPI_MISO_DDR &= ~_BV(SPI_MISO_PIN)
#define SPI_MISO_IS_HIGH ((SPI_MISO_IPR & _BV(SPI_MISO_PIN)) != 0x00)
#define SPI_MISO_IS_LOW ((SPI_MISO_IPR & _BV(SPI_MISO_PIN)) == 0x00)
/* SPI master out slave in*/
#define SPI_MOSI_PIN 14
#define SPI_MOSI_PORT PORTD
#define SPI_MOSI_IPR PIND
#define SPI_MOSI_DDR DDRD
#define SPI_MOSI_ON SPI_MOSI_PORT |= _BV(SPI_MOSI_PIN)
#define SPI_MOSI_OFF SPI_MOSI_PORT &= ~_BV(SPI_MOSI_PIN)
#define SPI_MOSI_OUTPUT SPI_MOSI_DDR |= _BV(SPI_MOSI_PIN)
#define SPI_MOSI_INPUT SPI_MOSI_DDR &= ~_BV(SPI_MOSI_PIN)
#define SPI_MOSI_IS_HIGH ((SPI_MOSI_IPR & _BV(SPI_MOSI_PIN)) != 0x00)
#define SPI_MOSI_IS_LOW ((SPI_MOSI_IPR & _BV(SPI_MOSI_PIN) == 0x00)
#define NOP __asm__("nop");

75
remote/src/spi.cpp

@ -0,0 +1,75 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
/********************/
/** SPI routines **/
/********************/
#include "spi.h"
#include "pins.h"
SPI::SPI()
{
}
void SPI::init(void)
{
SPI_SCLK_OUTPUT;
SPI_MOSI_OUTPUT;
SPI_MISO_INPUT;
}
void SPI::write(const uint8_t command)
{
uint8_t tmp = command;
//SCK start low
SPI_SCLK_OFF;
SPI_MOSI_OFF;
for (uint8_t i = 0; i< 8; ++i) {
// check if bit is set
if(tmp & 0x80) {
SPI_MOSI_ON;
} else {
SPI_MOSI_OFF;
}
// trigger clock
SPI_SCLK_ON;
// go to next bit
tmp = tmp << 1;
// reset clock
SPI_SCLK_OFF;
}
SPI_MOSI_ON;
}
uint8_t SPI::read(void)
{
uint8_t result = 0;
for(uint8_t i = 0; i < 8; i++) {
result = result << 1;
if(SPI_MISO_IS_HIGH) {
result |= 0x01;
}
// trigger clock
SPI_SCLK_ON;
// give chip some time to react
NOP;
// reset clock
SPI_SCLK_OFF;
}
return result;
}

28
remote/src/spi.h

@ -0,0 +1,28 @@
/*
This project is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Multiprotocol is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Multiprotocol. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef SPI_H
#define SPI_H
#include <Arduino.h>
class SPI {
private:
public:
SPI();
void init(void);
void write(uint8_t command);
uint8_t read(void);
};
#endif
Loading…
Cancel
Save