diff --git a/Multiprotocol/FrSkyX_cc2500.ino b/Multiprotocol/FrSkyX_cc2500.ino index b2a8ef5..9b91c84 100644 --- a/Multiprotocol/FrSkyX_cc2500.ino +++ b/Multiprotocol/FrSkyX_cc2500.ino @@ -27,7 +27,7 @@ uint8_t FrSkyX_RX_Seq ; struct t_FrSkyX_TX_Frame { uint8_t count; - uint8_t payload[6]; + uint8_t payload[8]; } ; // Store FrskyX telemetry struct t_FrSkyX_TX_Frame FrSkyX_TX_Frames[4] ; @@ -196,46 +196,58 @@ static void __attribute__((unused)) FrSkyX_build_packet() #ifdef SPORT_SEND if (FrSkyX_TX_IN_Seq!=0xFF) {//RX has replied at least once - debugln("R:%X,T:%X",FrSkyX_TX_IN_Seq,FrSkyX_TX_Seq); if (FrSkyX_TX_IN_Seq & 0x08) {//Request init - debugln("Init"); + //debugln("Init"); FrSkyX_TX_Seq = 0 ; for(uint8_t i=0;i<4;i++) FrSkyX_TX_Frames[i].count=0; // discard frames in current output buffer } else if (FrSkyX_TX_IN_Seq & 0x04) {//Retransmit the requested packet - debugln("Retr:%d",FrSkyX_TX_IN_Seq&0x03); + debugln("Retry:%d",FrSkyX_TX_IN_Seq&0x03); + packet[21] |= FrSkyX_TX_IN_Seq&0x03; + packet[22] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq&0x03].count; for (uint8_t i=23;i<23+FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq&0x03].count;i++) packet[i] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq&0x03].payload[i]; - packet[22] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq&0x03].count; - packet[21] |= FrSkyX_TX_IN_Seq&0x03; } else if ( FrSkyX_TX_Seq != 0x08 ) { - if(FrSkyX_TX_IN_Seq==FrSkyX_TX_Seq) + if(FrSkyX_TX_Seq==FrSkyX_TX_IN_Seq) {//Send packet from the incoming radio buffer + //debugln("Send:%d",FrSkyX_TX_Seq); + packet[21] |= FrSkyX_TX_Seq; uint8_t nbr_bytes=0; for (uint8_t i=23;i= SportTail ) + used += MAX_SPORT_BUFFER - SportHead ; + else + used -= SportHead ; + if ( used < (MAX_SPORT_BUFFER>>1) ) + DATA_BUFFER_LOW_off; + } FrSkyX_TX_Seq = ( FrSkyX_TX_Seq + 1 ) & 0x03 ; // Next iteration send next packet } else - {//Retransmit the last packet - debugln("Retr:%d",FrSkyX_TX_IN_Seq); - for (uint8_t i=23;i<23+FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].count;i++) - packet[i] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].payload[i]; - packet[22] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].count; + {//Not in sequence somehow, transmit what the receiver wants but why not asking for retransmit... + debugln("RX_Seq:%d,TX:%d",FrSkyX_TX_IN_Seq,FrSkyX_TX_Seq); packet[21] |= FrSkyX_TX_IN_Seq; + packet[22] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].count; + for (uint8_t i=23;i<23+FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].count;i++) + packet[i] = FrSkyX_TX_Frames[FrSkyX_TX_IN_Seq].payload[i-23]; } } else @@ -243,7 +255,7 @@ static void __attribute__((unused)) FrSkyX_build_packet() } if(packet[22]) {//Debug - debug("SPort_out: "); + debug("SP: "); for(uint8_t i=0;i SportTail ) + used += MAX_SPORT_BUFFER - SportHead ; + else + used -= SportHead ; + if ( used >= MAX_SPORT_BUFFER-(MAX_SPORT_BUFFER>>2) ) + { + DATA_BUFFER_LOW_on; + SEND_MULTI_STATUS_on; //Send Multi Status ASAP to inform the TX + debugln("Low buf=%d,h=%d,t=%d",used,SportHead,SportTail); + } } #endif } diff --git a/Multiprotocol/Telemetry.ino b/Multiprotocol/Telemetry.ino index c7db2b9..7487203 100644 --- a/Multiprotocol/Telemetry.ino +++ b/Multiprotocol/Telemetry.ino @@ -111,6 +111,8 @@ static void multi_send_status() break; } #endif + if(IS_DATA_BUFFER_LOW_on) + flags |= 0x80; } Serial_write(flags); @@ -353,7 +355,6 @@ void frsky_check_telemetry(uint8_t *packet_in,uint8_t len) } else {//Not in sequence - debugln("NS"); struct t_FrSkyX_RX_Frame *q ; uint8_t count ; // packet_in[4] RSSI @@ -725,9 +726,10 @@ void TelemetryUpdate() #if ( defined(MULTI_TELEMETRY) || defined(MULTI_STATUS) ) { uint32_t now = millis(); - if ((now - lastMulti) > MULTI_TIME) + if (IS_SEND_MULTI_STATUS_on || (now - lastMulti) > MULTI_TIME) { multi_send_status(); + SEND_MULTI_STATUS_off; lastMulti = now; return; }