M63 RC transmitter for RPi, uses XN297 transceiver on SPI port

m63.cpp

/*
 This program 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.
 This program 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.
 If not, see <http://www.gnu.org/licenses/>.
 */

 // M63 RC transmitter for RPi, uses XN297 transceiver on SPI port
 
#include <fcntl.h>            //Needed for SPI port
#include <sys/ioctl.h>        //Needed for SPI port
#include <linux/spi/spidev.h>	//Needed for SPI port
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <unistd.h>
#include <wiringPi.h> 
#include <wiringPiSPI.h>

#define CE_off digitalWrite(6, LOW)
#define CE_on  digitalWrite(6, HIGH)

#define PACKET_PERIOD 3800
#define PAYPLOAD_SIZE 9
#define PACKET_FREQ 0x2D

const unsigned char transmitterID[3] = {0xE2, 0x4D, 0x3C}; // as stock tx

const unsigned char channels[] = {
    0x14, 0x05, 0x14, 0x05, 0x14, 0x05, 0x2d, 0x43,
    0x2d, 0x4b, 0x28, 0x4b, 0x3d, 0x0e, 0x3d, 0x0e,
    0x3d, 0x0e, 0x28, 0x4b, 0x28, 0x43, 0x2d, 0x43
};

const unsigned char mystery_byte[] = {
    0x00, 0x10, 0x20, 0x01, 0x11, 0x21, 0x02, 0x12,
    0x22, 0x03, 0x13, 0x23, 0x04, 0x14, 0x24, 0x05,
    0x15, 0x25, 0x06, 0x16, 0x26, 0x07, 0x17, 0x27
};
    
enum {
    // flags going to packet[6]
    // FLAG_RATE0, // default rate, no flag
    FLAG_RATE1   = 0x01,
    FLAG_RATE2   = 0x02,
    FLAG_VIDEO   = 0x10,
    FLAG_SNAPSHOT= 0x20,
    FLAG_FLIP    = 0x80,
};

static unsigned char tx_addr[5];
static unsigned char checksum_offset;
static unsigned char packet[PAYPLOAD_SIZE];

void initTXID()
{
    checksum_offset = (transmitterID[0] + transmitterID[1] + transmitterID[2]) & 0xff;
}

inline void set_rf_channel(unsigned char channel)
{
    const unsigned char command = 0x25;
    unsigned char buffer[2] = {command, channel & 0x7f};
    wiringPiSPIDataRW(0, buffer, sizeof(buffer)); // set RF channel
}

void set_tx_address(unsigned char *address, unsigned int len)
{
    const unsigned char command = 0x30;
    unsigned char buffer[6];
    buffer[0] = command;
    if(len > 5) {
        len = 5;
    }
    for(int i=0; i<len; i++) {
        buffer[i+1] = address[i];
    }
    wiringPiSPIDataRW(0, buffer, len+1); // set tx address
}

inline void flush_tx()
{
    unsigned char command_buffer[2];
    command_buffer[0] = 0x27;
    command_buffer[1] = 0x70;
    wiringPiSPIDataRW(0, command_buffer, 2); // clear data ready, data sent, and retransmit
    command_buffer[0] = 0xe1;
    command_buffer[1] = 0x00;
    wiringPiSPIDataRW(0, command_buffer, 2); // flush tx
}

void init()
{
    std::cout << "Init XN297\n";
    initTXID();
    delay(10);
    for(unsigned char i=0; i<5; i++)
        tx_addr[i] = 0xCC;
    
    wiringPiSPIDataRW(0, (unsigned char *) "\x3F\x4C\x84\x6F\x9C\x20", 6); // Set Baseband parameters (debug registers) - BB_CAL
    wiringPiSPIDataRW(0, (unsigned char *) "\x3E\xC9\x9A\xB0\x61\xBB\xAB\x9C", 8); // Set RF parameters (debug registers) - RF_CAL
    wiringPiSPIDataRW(0, (unsigned char *) "\x39\x0B\xDF\xC4\xA7\x03", 6); // Set Demodulator parameters (debug registers) - DEMOD_CAL
    flush_tx();
    set_tx_address(tx_addr, 5); // set tx address for bind
    wiringPiSPIDataRW(0, (unsigned char *) "\x2A\xCC\xCC\xCC\xCC\xCC", 6); // set rx address (useless)    
    wiringPiSPIDataRW(0, (unsigned char *) "\xE2\x00", 2); // flush RX fifo (useless)
    wiringPiSPIDataRW(0, (unsigned char *) "\x21\x00", 2); // no Auto Ack
    wiringPiSPIDataRW(0, (unsigned char *) "\x22\x01", 2); // enable data pipe 0 only 
    wiringPiSPIDataRW(0, (unsigned char *) "\x23\x03", 2); // 5 bytes address
    set_rf_channel(PACKET_FREQ); // set RF channel 
    wiringPiSPIDataRW(0, (unsigned char *) "\x24\x00", 2); // no auto retransmit
    wiringPiSPIDataRW(0, (unsigned char *) "\x31\x09", 2); // set RX pipe 0 buffer length (useless)
    wiringPiSPIDataRW(0, (unsigned char *) "\x26\x07", 2); // 1Mbps, max RF power output
    wiringPiSPIDataRW(0, (unsigned char *) "\x50\x73", 2); // Activate extra feature register
    wiringPiSPIDataRW(0, (unsigned char *) "\x3C\x00", 2); // Disable dynamic payload length
    wiringPiSPIDataRW(0, (unsigned char *) "\x3D\x00", 2); // Extra features all off
    wiringPiSPIDataRW(0, (unsigned char *) "\x1D\x00", 2); // read reg 1D back ? no MISO ...
    delay(150);
    wiringPiSPIDataRW(0, (unsigned char *) "\x20\x0E", 2); // power on, transmit mode, 2 byte CRC
    delay(100);
}

void write_payload(unsigned char* data, unsigned int len)
{
    const unsigned char command = 0x0a;
    unsigned char buffer[33];
    buffer[0] = command;
    if(len > 32) {
        len = 32;
    }
    for(int i=0; i<len; i++) {
        buffer[i+1] = data[i];
    }
    CE_off;
    wiringPiSPIDataRW(0, buffer, len+1); // write tx fifo
    CE_on; // transmit
}

void bind()
{
    std::cout << "Binding " << std::flush;
    unsigned int counter = 128;
    while(counter--) {
        packet[0] = 0x20; // fixed (firmware date 2014-07-23 ?)
        packet[1] = 0x14; // fixed
        packet[2] = 0x07; // fixed
        packet[3] = 0x23; // fixed
        packet[4] = transmitterID[0]; // 1st byte for data phase tx address  
        packet[5] = transmitterID[1]; // 2nd byte for data phase tx address 
        packet[6] = transmitterID[2]; // 3rd byte for data phase tx address
        packet[7] = checksum_offset; // checksum offset
        packet[8] = 0xAA; // fixed
        CE_off;
        set_rf_channel(PACKET_FREQ);
        CE_on;
        delayMicroseconds(17);
        CE_off;
        flush_tx();
        set_rf_channel(PACKET_FREQ);
        write_payload(packet, PAYPLOAD_SIZE); //(bind packet)
        delayMicroseconds(PACKET_PERIOD);
        if(!(counter%10)) {
            std::cout << "." << std::flush;
        }
    }
    delay(15);
    tx_addr[0] = transmitterID[0];
    tx_addr[1] = transmitterID[1];
    tx_addr[2] = transmitterID[2];
    tx_addr[3] = 0xCC;
    tx_addr[4] = 0xCC;
    set_tx_address(tx_addr, 5);
}

long map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void send_packet(unsigned char throttle, unsigned char aileron, unsigned char elevator, unsigned char rudder, unsigned char flags)
{
    CE_off;
    static int sequence = 15;
    static bool first_packet=true;
    packet[0] = map(throttle, 0, 255, 0xE1, 0x00);
    packet[1] = map(rudder, 0, 255, 0xE1, 0x00);
    packet[2] = map(aileron, 0, 255, 0xE1, 0x00);
    packet[3] = map(elevator, 0, 255, 0xE1, 0x00);
    packet[4] = 0x20; // elevator trim, range 0x00-0x3f
    packet[5] = 0x20; // aileron trim
    packet[6] = flags;
    packet[7] = mystery_byte[sequence];
    packet[8] = checksum_offset;
    for(int i=0; i<PAYPLOAD_SIZE-1; i++) {
        packet[8] += packet[i];
    }
    set_rf_channel(first_packet ? PACKET_FREQ : channels[sequence]);
    CE_on;
    delayMicroseconds(5);
    CE_off;
    flush_tx();
    set_rf_channel(PACKET_FREQ);
    write_payload( packet, PAYPLOAD_SIZE);
    if(++sequence >= sizeof(mystery_byte)) {
        sequence = 0;
    }
    first_packet = false;
}

int main()
{
    std::cout << "Don't forget to \"gpio load spi\" in linux kernel before use\nStarting M68 transmitter\n";
    wiringPiSetup();
    // Setup CE as GPIO 6
    pinMode(6, OUTPUT); // Setup CE as GPIO 6
    CE_off;
    wiringPiSPISetup(0,140000); // Initialise SPI module
    delay(100); // Delay 100ms
    init(); // init xn297
    bind(); // bind to M63
    std::cout << "\nRunning ";
    // test loop
    for(int loop=0; loop<5; loop++) {
        std::cout << "." << std::flush;
        for(int throttle=0; throttle < 256; throttle++) {
            int counter = 2;
            while(counter--) {
                send_packet( throttle, 129, 129, 129, 0);
                delayMicroseconds(PACKET_PERIOD);
            }
        }
        std::cout << "." << std::flush;
        for(int throttle=256; throttle > 0; throttle--) {
            int counter = 2;
            while(counter--) {
                send_packet( throttle-1, 129, 129, 129, 0);
                delayMicroseconds(PACKET_PERIOD);
            }
        }
    }
    std::cout << "\nDone \n";
}

Makefile

DEBUG   = -O3
CC      = g++ -std=c++
INCLUDE = -I/usr/local/include
CFLAGS  = $(DEBUG) -Wall $(INCLUDE) -Winline -pipe

LDFLAGS = -L/usr/local/lib
LDLIBS    = -lwiringPi -lwiringPiDev -lpthread -lm
SRC     =       m63.cpp
OBJ     =       $(SRC:.cpp=.o)
BINS    =       $(SRC:.cpp=)

all:    $(BINS)

m63:    m63.o
        @echo [link]
        @$(CC) -o $@ m63.o $(LDFLAGS) $(LDLIBS)

.c.o:
        @echo [CC] $<
        @$(CC) -c $(CFLAGS) $< -o $@

clean:
        @echo "[Clean]"
        @rm -f $(OBJ) *~ core tags $(BINS)

tags:   $(SRC)
        @echo [ctags]
        @ctags $(SRC)

depend:
        makedepend -Y $(SRC)