AFHDS2A_a7105.cpp

 /*
 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/>.
 */
// Last sync with hexfet new_protocols/flysky_a7105.c dated 2015-09-28
#include "er9x.h"
#include "iface_a7105.h"

#ifdef AFHDS2A_A7105_INO

#define AFHDS2A_TXPACKET_SIZE	37
#define AFHDS2A_RXPACKET_SIZE	37
#define AFHDS2A_HUB_TELEMETRY
//#define AFHDS2A_NUMFREQ			16


//#define GIO1 GIO1_GetVal(GIO1_DeviceData)

 static void AFHDS2A_calc_channels() {
     uint8_t idx = 0;
     uint32_t rnd = ID.MProtocol_id;
     uint8_t i;
     while (idx < AFHDS2A_NUMFREQ)
     {
         uint8_t band_no = ((((idx << 1) | ((idx >> 1) & 0b01)) + ID.rx_tx_addr[3]) & 0b11);
         rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization

         uint8_t next_ch = band_no * 41 + 1 + ((rnd >> idx) % 41); // Channel range: 1..164

         for (i = 0; i < idx; i++)
         {
             // Keep the distance 5 between the channels
             uint8_t distance;
             if (next_ch > hopping_frequency[i])
                 distance = next_ch - hopping_frequency[i];
             else
                 distance = hopping_frequency[i] - next_ch;

             if (distance < 5)
                 break;
         }

         if (i != idx)
             continue;

         hopping_frequency[idx++] = next_ch;
     }
 }

// telemetry sensors ID
enum{
	AFHDS2A_SENSOR_RX_VOLTAGE   = 0x00,
	AFHDS2A_SENSOR_RX_TEMP      = 0x01,
	AFHDS2A_SENSOR_RX_RPM       = 0x02,
	AFHDS2A_SENSOR_A3_VOLTAGE   = 0x03,
	AFHDS2A_SENSOR_RX_ERR_RATE  = 0xfe,
	AFHDS2A_SENSOR_RX_RSSI      = 0xfc,
	AFHDS2A_SENSOR_RX_NOISE     = 0xfb,
	AFHDS2A_SENSOR_RX_SNR       = 0xfa,
};

void AFHDS2A_update_telemetry()
{
    if (packet[0] == 0xAA && packet[9] == 0xFD)
        return; // ignore packets which contain the RX configuration: FD FF 32 00 01 00 FF FF FF 05 DC 05 DE FA FF FF FF FF FF FF FF FF FF FF FF FF FF FF
                // Read TX RSSI

    /*
	int16_t temp=256-(A7105_ReadReg(A7105_1D_RSSI_THOLD)*8)/5;		// value from A7105 is between 8 for maximum signal strength to 160 or less
	if(temp<0) temp=0;
	else if(temp>255) temp=255;
	TX_RSSI=temp;
*/

    // AA | TXID | rx_id | sensor id | sensor # | value 16 bit big endian | sensor id ......
    // AC | TXID | rx_id | sensor id | sensor # | length | bytes | sensor id ......
    if (packet[0] == 0xAA)
    { // 0xAA Normal telemetry, 0xAC Extended telemetry not decoded here
        for (uint8_t sensor = 0; sensor < 7; sensor++)
        {
            // Send FrSkyD telemetry to TX
            uint8_t index = 9 + (4 * sensor);
            switch (packet[index])
            {
            case AFHDS2A_SENSOR_RX_VOLTAGE:
                telem_AFHDS2A[RX_IntV] = packet[index + 3] << 8 | packet[index + 2];
                telem_status |= (1 << RX_IntV);
                break;
            case AFHDS2A_SENSOR_A3_VOLTAGE:
                telem_AFHDS2A[RX_ExtV] = (packet[index + 3] << 8) | (packet[index + 2]);
                telem_status |= (1 << RX_ExtV);
                break;
            case AFHDS2A_SENSOR_RX_TEMP:
                telem_AFHDS2A[RX_Temp] = (packet[index + 3] << 8) | (packet[index + 2]);
                telem_status |= (1 << RX_Temp);
                break;
            case AFHDS2A_SENSOR_RX_RPM:
                telem_AFHDS2A[RX_RPM] = (packet[index + 3] << 8) | (packet[index + 2]);
                telem_status |= (1 << RX_RPM);
                break;
            case AFHDS2A_SENSOR_RX_ERR_RATE:
                telem_AFHDS2A[RX_Err] = packet[index + 2];
                telem_status |= (1 << RX_Err);
                break;
            case AFHDS2A_SENSOR_RX_RSSI:
                //RX_RSSI = -packet[index+2];
                break;
            case 0xff: // end of data
                return;
                /*default:
		// unknown sensor ID
		break;*/
            }
        }
    }
}

static void AFHDS2A_build_bind_packet(void)
{
	uint8_t ch;
    uint8_t phase = RadioState & 0x0F;
	memcpy( &packet[1], ID.rx_tx_addr, 4);
	memset( &packet[5], 0xff, 4);
	packet[10]= 0x00;
	for(ch=0; ch<AFHDS2A_NUMFREQ; ch++)
		packet[11+ch] = hopping_frequency[ch];
	memset( &packet[27], 0xff, 10);
	packet[37] = 0x00;
	switch(phase)
	{
		case AFHDS2A_BIND1:
			packet[0] = 0xbb;
			packet[9] = 0x01;
			break;
		case AFHDS2A_BIND2:
		case AFHDS2A_BIND3:
		case AFHDS2A_BIND4:
			packet[0] = 0xbc;
			if(phase == AFHDS2A_BIND4)
			{
				memcpy( &packet[5], &g_model.rxID, 4);
				memset( &packet[11], 0xff, 16);
			}
			packet[9] = phase-1;
			if(packet[9] > 0x02)
				packet[9] = 0x02;
			packet[27]= 0x01;
			packet[28]= 0x80;
			break;
	}
}

void AFHDS2A_build_packet(uint8_t type)
{
//	uint16_t val;
	memcpy( &packet[1], ID.rx_tx_addr, 4);
	memcpy( &packet[5], g_model.rxID, 4);
	switch(type)
	{
	case AFHDS2A_PACKET_STICKS:
		packet[0] = 0x58;
		for (uint8_t ch = 0; ch < 14; ch++) {
			uint16_t channelMicros;
			if (g_model.failsafeRepeat)
				channelMicros = convert_failsafe_ppm(ch);
			else
			channelMicros = g_chans512[ch] / 2 + RADIO_PPM_CENTER;
			packet[9 + ch * 2] = channelMicros & 0xFF;
			packet[10 + ch * 2] = (channelMicros >> 8) & 0xFF;
		}
			#ifdef AFHDS2A_LQI_CH
				// override channel with LQI
				val = 2000 - 10*RX_LQI;
				packet[9+((AFHDS2A_LQI_CH-1)*2)] = val & 0xff;
				packet[10+((AFHDS2A_LQI_CH-1)*2)] = (val >> 8) & 0xff;
			#endif
			break;
		case AFHDS2A_PACKET_FAILSAFE:
			packet[0] = 0x56;
			for(uint8_t ch=0; ch<14; ch++)
			{
					if(g_model.failsafeMode)
					{ // Failsafe values
						uint16_t failsafeMicros = convert_failsafe_ppm(ch);
						packet[9 + ch*2] =  failsafeMicros & 0xff;
						packet[10+ ch*2] = ( failsafeMicros >> 8) & 0xff;
					}
					else
					{ // no values
						packet[9 + ch*2] = 0xff;
						packet[10+ ch*2] = 0xff;
					}
			}
			break;
		case AFHDS2A_PACKET_SETTINGS:
			packet[0] = 0xaa;
			packet[9] = 0xfd;
			packet[10]= 0xff;
			if(g_model.ServoFreq < 50 || g_model.ServoFreq>400) g_model.ServoFreq = 50;	// default is 50Hz
			packet[11]= g_model.ServoFreq;
			packet[12]= g_model.ServoFreq >> 8;
			if(g_model.PPMOut)
				packet[13] = 0x01;	// PPM output enabled
			else
				packet[13] = 0x00;
			packet[14]= 0x00;
			for(uint8_t i=15; i<37; i++)
				packet[i] = 0xff;
			packet[18] = 0x05;		// ?
			packet[19] = 0xdc;		// ?
			packet[20] = 0x05;		// ?
			if(g_model.IS_BUS)
				packet[21] = 0xdd;	// SBUS output enabled
			else
				packet[21] = 0xde;	// IBUS
			break;
	}
	if (hopping_frequency_no >= AFHDS2A_NUMFREQ)
		packet[37] = 0x00;
	else
		packet[37] = 0;//hopping_frequency_no+2;
}

void ActionAFHDS2A(void) {
  uint8_t Channel;
  static uint8_t packet_type;
  static uint16_t telem_counter;
  static uint16_t packet_counter = 0;
  A7105_AdjustLOBaseFreq();

  //----------------------------------------------------------------------------
  if (IS_BIND_DONE)
    RadioState = (RadioState & 0xF0) | AFHDS2A_DATA;
  switch (RadioState) {
  case ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND1)):
  case ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND2)):
  case ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND3)):
  case ((TIM_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND1)):
  case ((TIM_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND2)):
  case ((TIM_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND3)):
    goto SendBIND_;

  case ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND4)):
    goto SendBIND4_;

  case ((GPIO_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND1)):
  case ((GPIO_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND2)):
  case ((GPIO_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_BIND3)):
    goto EndSendBIND123_;

  case ((GPIO_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND1)):
  case ((GPIO_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND2)):
  case ((GPIO_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_BIND3)):
    goto ResBIND123_;

  case ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_DATA)):
  case ((TIM_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_DATA)):
    goto SendData_;

  case ((GPIO_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_DATA)):
    goto EndSendData_;

  case ((GPIO_CALL << CALLER) | (RES << SEND_RES) | (AFHDS2A_DATA)):
    goto ResData_;

  default:
    return;
  }
//--------------------------------------------------------------------------
SendBIND4_: //--------------------------------------------------------------
  bind_phase++;
  if (bind_phase >= 4) {
    hopping_frequency_no = 1;
    RadioState = (RadioState & 0xF0) | AFHDS2A_DATA;
    SETBIT(RadioState, SEND_RES, SEND);
    BIND_DONE;
    return;
  }
SendBIND_: //--------------------------------------------------------------
  AFHDS2A_build_bind_packet();
  Channel = (packet_count % 2 ? 0x0d : 0x8c);
  SETBIT(RadioState, SEND_RES, SEND);
  goto Send_;
EndSendBIND123_: //-----------------------------------------------------------
  A7105_SetPower();
  A7105_SetTxRxMode(TXRX_OFF); // Turn LNA off since we are in near range and we want to prevent swamping
  A7105_Strobe(A7105_RX);
  EnableGIO();
  RadioState++;
  if ((RadioState & 0x0F) > AFHDS2A_BIND3)
    RadioState = (RadioState & 0xF0) | AFHDS2A_BIND1;
  SETBIT(RadioState, SEND_RES, RES);
  return;
ResBIND123_: //-----------------------------------------------------------
  A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
  if ((packet[0] == 0xbc) & (packet[9] == 0x01)) {
    for (uint8_t i = 0; i < 4; i++) {
      g_model.rxID[i] = packet[5 + i];
    }
    RadioState = (RadioState & 0xF0) | AFHDS2A_BIND4;
    bind_phase = 0;
    SETBIT(RadioState, SEND_RES, SEND);
  }
  return;
SendData_: //--------------------------------------------------------------
  Channel = hopping_frequency[hopping_frequency_no++];
  AFHDS2A_build_packet(packet_type);
  SETBIT(RadioState, SEND_RES, SEND);
  if (hopping_frequency_no >= AFHDS2A_NUMFREQ) {
    hopping_frequency_no = 0;
    goto SendNoAntSwitch_;
  }
  goto Send_;
EndSendData_: //-----------------------------------------------------------
  A7105_SetPower();
  A7105_SetTxRxMode(RX_EN);
  A7105_Strobe(A7105_RX);
  if (!(packet_counter % 1569))
    packet_type = AFHDS2A_PACKET_FAILSAFE;
  else
    packet_type = AFHDS2A_PACKET_STICKS;
  SETBIT(RadioState, SEND_RES, RES);
  EnableGIO();
  if (telem_counter < 100)
    telem_counter++;
  else
    telem_status = 0;
  SETBIT(RadioState, SEND_RES, RES);
  return;
ResData_: //-----------------------------------------------------------
  A7105_ReadData(AFHDS2A_RXPACKET_SIZE);
  if (packet[0] == 0xAA && packet[9] == 0xFC) // RX is asking for settings
    packet_type = AFHDS2A_PACKET_SETTINGS;       
  if (packet[0] == 0xAA && packet[9] == 0xFD) // RX is asking for FailSafe
    packet_type = AFHDS2A_PACKET_FAILSAFE;       
  if (packet[0] == 0xAA || packet[0] == 0xAC) {
    if (!memcmp(&packet[1], ID.rx_tx_addr, 4)) { // Validate TX address
      AFHDS2A_update_telemetry();
      telem_counter = 0;
    }
  }
  SETBIT(RadioState, SEND_RES, SEND);
  return;
Send_: //---------------------------------------------------------------
  A7105_AntSwitch();
SendNoAntSwitch_:
  A7105_WriteData(AFHDS2A_TXPACKET_SIZE, Channel);
  EnableGIO();
  packet_count++;
  packet_counter++;
  return;
}

/******************************************************************************/
/*                                                                            */
/******************************************************************************/
void initAFHDS2A() {
  protocol        = PROTO_AFHDS2A;
  RadioState      = ((TIM_CALL << CALLER) | (SEND << SEND_RES) | (AFHDS2A_DATA));
  ID.MProtocol_id = GetChipID();
  AFHDS2A_calc_channels();
  A7105_Init();
  packet_count         = 0;
  hopping_frequency_no = 0;
}
#endif