diff --git a/Multiprotocol/CX10_nrf24l01.ino b/Multiprotocol/CX10_nrf24l01.ino index bcf5c2e..2c649b4 100644 --- a/Multiprotocol/CX10_nrf24l01.ino +++ b/Multiprotocol/CX10_nrf24l01.ino @@ -57,21 +57,14 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) packet[3] = rx_tx_addr[2]; packet[4] = rx_tx_addr[3]; // packet[5] to [8] (aircraft id) is filled during bind for blue board - packet[5+offset] = lowByte(Servo_data[AILERON]); - packet[6+offset]= highByte(Servo_data[AILERON]); - packet[7+offset]= lowByte(Servo_data[ELEVATOR]); - packet[8+offset]= highByte(Servo_data[ELEVATOR]); + uint16_t aileron=Servo_data[AILERON]; + uint16_t elevator=3000-Servo_data[ELEVATOR]; + uint16_t rudder=3000-Servo_data[RUDDER]; packet[9+offset]= lowByte(Servo_data[THROTTLE]); packet[10+offset]= highByte(Servo_data[THROTTLE]); - packet[11+offset]= lowByte(Servo_data[RUDDER]); - packet[12+offset]= highByte(Servo_data[RUDDER]); - - // Channel 5 - flip flag - if(Servo_AUX1) - packet[12+offset] |= CX10_FLAG_FLIP; // flip flag - //flags=0; // packet 13 - uint8_t flags2=0; // packet 14 + // Channel 5 - flip flag + packet[12+offset] = GET_FLAG(Servo_AUX1,CX10_FLAG_FLIP); // flip flag applied on rudder // Channel 6 - rate mode is 2 lsb of packet 13 if(Servo_data[AUX2] > PPM_MAX_COMMAND) // rate 3 / headless on CX-10A @@ -81,6 +74,7 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) flags = 0x00; // rate 1 else flags = 0x01; // rate 2 + uint8_t flags2=0; // packet 14 uint8_t video_state=packet[14] & 0x21; switch(sub_protocol) @@ -92,6 +86,8 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) case Q282: case Q242: case Q222: + aileron = 3000 - aileron; + rudder = 3000 - rudder; memcpy(&packet[15], "\x10\x10\xaa\xaa\x00\x00", 6); //FLIP|LED|PICTURE|VIDEO|HEADLESS|RTH|XCAL|YCAL flags2 = GET_FLAG(Servo_AUX1, 0x80) // Channel 5 - FLIP @@ -123,17 +119,19 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) else { // Q222 flags=0; - packet[12] &= ~CX10_FLAG_FLIP; } if(Servo_AUX6) flags |=0x80; // Channel 10 - RTH break; case DM007: + aileron = 3000 - aileron; //FLIP|MODE|PICTURE|VIDEO|HEADLESS flags2= GET_FLAG(Servo_AUX3,CX10_FLAG_SNAPSHOT) // Channel 7 - picture |GET_FLAG(Servo_AUX4,CX10_FLAG_VIDEO); // Channel 8 - video if(Servo_AUX5) flags |= CX10_FLAG_HEADLESS; // Channel 9 - headless break; case JC3015_2: + aileron = 3000 - aileron; + elevator = 3000 - elevator; //FLIP|MODE|LED|DFLIP if(Servo_AUX4) packet[12] &= ~CX10_FLAG_FLIP; case JC3015_1: @@ -142,6 +140,7 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) |GET_FLAG(Servo_AUX4,_BV(4)); // Channel 8 break; case MK33041: + elevator = 3000 - elevator; //FLIP|MODE|PICTURE|VIDEO|HEADLESS|RTH flags|=GET_FLAG(Servo_AUX3,_BV(7)) // Channel 7 - picture |GET_FLAG(Servo_AUX6,_BV(2)); // Channel 10 - rth @@ -149,9 +148,15 @@ static void __attribute__((unused)) CX10_Write_Packet(uint8_t bind) |GET_FLAG(Servo_AUX5,_BV(5)); // Channel 9 - headless break; } + packet[5+offset] = lowByte(aileron); + packet[6+offset]= highByte(aileron); + packet[7+offset]= lowByte(elevator); + packet[8+offset]= highByte(elevator); + packet[11+offset]= lowByte(rudder); + packet[12+offset]|= highByte(rudder); packet[13+offset]=flags; packet[14+offset]=flags2; - + // Power on, TX mode, 2byte CRC // Why CRC0? xn297 does not interpret it - either 16-bit CRC or nothing XN297_Configure(_BV(NRF24L01_00_EN_CRC) | _BV(NRF24L01_00_CRCO) | _BV(NRF24L01_00_PWR_UP));