File content as of revision 2:ce397d92f68e:

#include "mbed.h"
#include "nRF24L01P.h"

#define CMD_VALUE 65535
#define CMD_GET_STATUS 1
#define CMD_INITIALIZE 2

#define RCV_TIMEOUT_MS 1000

#define SERVO_MAX 2200
#define SERVO_MIN 800
#define SERVO_CENTER (SERVO_MAX-SERVO_MIN)/2+SERVO_MIN

#define CHANNELS_NUMBER 12 
#define CHANNEL_MAX 1024
#define CHANNEL_MIN 0
#define CHANNEL_DEPTH       3
#define CHANNEL_YAW         1
#define CHANNEL_THROTTLE    0

#define STATE_INITILIZATION 0x00
#define STATE_RUN 0x01
#define STATE_FAILSAFE 0x02

Serial pc(USBTX, USBRX); // tx, rx
 
nRF24L01P radio(D11, D12, D13, D10, D9, D8);    // mosi, miso, sck, csn, ce, irq

uint16_t channels[CHANNELS_NUMBER];
uint16_t buffer  [CHANNELS_NUMBER]; 
int bytesWritten;

uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION;


PwmOut depthEsc(PA_10);
PwmOut leftEsc(PA_8);
PwmOut rightEsc(PA_11);


Timer t;

void setState(uint8_t newState) {
    state=state | newState;
}

void initialize() {
    state=((uint16_t)0x01 << 8) | state;
}

bool isInitializated() {
    return (state>>8) & 0x01;
}

void zeroBuffer(){
    memset(buffer, 0, sizeof(buffer));     
}

float normToPWM(float chNorm){
    return chNorm*(SERVO_MAX-SERVO_MIN)+SERVO_MIN;
}
float normChannel(uint16_t channel){
    return ((float)channel-CHANNEL_MIN)/(CHANNEL_MAX-CHANNEL_MIN);
}
float chToPWM(uint16_t channel){
    return normToPWM(normChannel(channel));
}

void channelsToEsc() {
    if (!isInitializated()) {
        pc.printf("WARNING!!!! NOT INITIALIZATED!!!!\r\n");
        return;    
    }
    depthEsc.pulsewidth_us(chToPWM(channels[CHANNEL_DEPTH])); // servo position determined by a pulsewidth between 1-2ms
    
    float forwardFactor = (1.0f-normChannel(channels[CHANNEL_THROTTLE]))-0.5f;
    float yawFactor     = normChannel(channels[CHANNEL_YAW])-0.5f;
    pc.printf("\t\t\t\tff: %f; yf: %f\r\n", forwardFactor, yawFactor);
    leftEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500+yawFactor*500);
    rightEsc.pulsewidth_us(SERVO_CENTER+forwardFactor*500-yawFactor*500);
}

int main() {
    
    depthEsc.period_us(20000);          // servo requires a 20ms period
    depthEsc.pulsewidth_us(SERVO_CENTER);
    leftEsc.period_us(20000);          // servo requires a 20ms period
    leftEsc.pulsewidth_us(SERVO_CENTER);
    rightEsc.period_us(20000);          // servo requires a 20ms period
    rightEsc.pulsewidth_us(SERVO_CENTER);
    
    
    //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms
    
    //radio.powerUp();
    pc.baud(115200);
    pc.printf("We all live in a yellow submarine!\r\n");
    
    radio.setRxAddress(0xDEADBEEF0F);
    radio.setTxAddress(0xDEADC0DE0F);
    radio.setRfFrequency(2405);
    radio.powerUp();
 
    pc.printf( "nRF24L01+ Frequency    : %d MHz\r\n",  radio.getRfFrequency() );
    pc.printf( "nRF24L01+ Output power : %d dBm\r\n",  radio.getRfOutputPower() );
    pc.printf( "nRF24L01+ Data Rate    : %d kbps\r\n", radio.getAirDataRate() );
    pc.printf( "nRF24L01+ TX Address   : 0x%010llX\r\n", radio.getTxAddress() );
    pc.printf( "nRF24L01+ RX Address   : 0x%010llX\r\n", radio.getRxAddress() );
    
    /*while (radio.getRxAddress()!=0xDEADBEEF0F){
        pc.printf("RX address fail!\r\n");
        radio.powerDown();
        radio.setRxAddress(0xDEADBEEF0F);
        radio.powerUp();
    }*/
 
    radio.setTransferSize( CHANNELS_NUMBER*sizeof(uint16_t) );
 
    radio.setReceiveMode();
 
    radio.enable();
    
    pc.printf("Greetings, capitain!\r\n");
    pc.printf("is initializated: %s\r\n", isInitializated()?"true":"false");
    pc.printf( "0x%X\r\n", state );
    t.start();
    
    while (1)
    {
        int startTime=t.read_ms();
        while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS);
        
        if(!radio.readable()) {
            //failsafe
            setState(STATE_FAILSAFE);
            pc.printf("Failsafe\n\r", channels[0],channels[1],channels[2],channels[3]);
            channels[0]=512;
            channels[1]=512;
            channels[2]=512;
            channels[3]=512;
        } else {
            int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
            
            if (buffer[0] == CMD_VALUE) {
                pc.printf("Command: %d\r\n", buffer[1]);
                switch(buffer[1]){
                    case CMD_GET_STATUS:
                        zeroBuffer();
                        //strcpy((char*)buffer,"I'm alive");
                        buffer[0]=state;
                        buffer[1]=t.read_us();
                        radio.setTransmitMode();
                        //wait(0.1);
                        bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
                        if (bytesWritten<2*CHANNELS_NUMBER){
                            pc.printf("Transmit error\r\n");
                        } else {
                            //pc.printf("Sent telemetry: %s\r\n",(char*)buffer);
                        }
                        radio.setReceiveMode();
                        //wait(0.1);
                    break;
                    case CMD_INITIALIZE:
                        initialize();
                        setState(STATE_RUN);
                    break;
                }    
            } else {
                for(int i=0; i<CHANNELS_NUMBER; i++){
                    channels[i] = buffer[i];
                }
                channelsToEsc();
                pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]);
            }
        }
        
        
    }
    
}