static void
scale_channels(const stp_vars_t *v, unsigned *zero_mask)
{
stpi_channel_group_t *cg = get_channel_group(v);
int i, j;
int physical_channel = 0;
if (!cg)
return;
if (zero_mask)
*zero_mask = 0;
for (i = 0; i < cg->channel_count; i++)
{
stpi_channel_t *ch = &(cg->c[i]);
if (ch->subchannel_count > 0)
for (j = 0; j < ch->subchannel_count; j++)
{
if (cg->gloss_channel != i)
{
stpi_subchannel_t *sch = &(ch->sc[j]);
unsigned density = sch->s_density;
unsigned short *output = cg->output_data + physical_channel;
if (density == 0)
{
clear_channel(output, cg->width, cg->total_channels);
if (zero_mask)
*zero_mask |= 1 << physical_channel;
}
else if (density != 65535)
{
if (scale_channel(output, cg->width, cg->total_channels,
density) == 0)
if (zero_mask)
*zero_mask |= 1 << physical_channel;
}
else if (zero_mask)
{
if (scan_channel(output, cg->width, cg->total_channels)==0)
*zero_mask |= 1 << physical_channel;
}
}
physical_channel++;
}
}
}
static void send_packet(u8 bind)
{
union {
u16 value;
struct {
u8 lsb;
u8 msb;
} bytes;
} chanval;
if (bind) {
if (telemetry)
packet[0] = 0xa3;
else
packet[0] = 0xa4;
memcpy(&packet[1], rx_tx_addr, 5);
memcpy(&packet[6], rf_channels, 4);
switch (Model.proto_opts[PROTOOPTS_FORMAT]) {
case FORMAT_REGULAR:
packet[10] = txid[0];
packet[11] = txid[1];
break;
case FORMAT_X16_AH:
packet[10] = 0x00;
packet[11] = 0x00;
break;
case FORMAT_IRDRONE:
packet[10] = 0x30;
packet[11] = 0x01;
break;
}
} else {
switch (Model.proto_opts[PROTOOPTS_FORMAT]) {
case FORMAT_REGULAR:
packet[0] = 0xa5;
break;
case FORMAT_X16_AH:
case FORMAT_IRDRONE:
packet[0] = 0xa6;
break;
}
packet[1] = 0xfa; // normal mode is 0xf7, expert 0xfa
packet[2] = GET_FLAG(CHANNEL_FLIP, 0x08)
| GET_FLAG(CHANNEL_HEADLESS, 0x02)
| GET_FLAG(CHANNEL_RTH, 0x01)
| GET_FLAG(CHANNEL_VIDEO, 0x10)
| GET_FLAG(CHANNEL_PICTURE, 0x20);
packet[3] = GET_FLAG(CHANNEL_INVERTED, 0x80)
| GET_FLAG(CHANNEL_TO, 0x20)
| GET_FLAG(CHANNEL_EMGSTOP, 0x04);
chanval.value = scale_channel(CHANNEL1, 0x3ff, 0); // aileron
packet[4] = chanval.bytes.msb + DYNTRIM(chanval.value);
packet[5] = chanval.bytes.lsb;
chanval.value = scale_channel(CHANNEL2, 0, 0x3ff); // elevator
packet[6] = chanval.bytes.msb + DYNTRIM(chanval.value);
packet[7] = chanval.bytes.lsb;
chanval.value = scale_channel(CHANNEL3, 0, 0x3ff); // throttle
packet[8] = chanval.bytes.msb + 0x7c;
packet[9] = chanval.bytes.lsb;
chanval.value = scale_channel(CHANNEL4, 0x3ff, 0); // rudder
packet[10] = chanval.bytes.msb + DYNTRIM(chanval.value);
packet[11] = chanval.bytes.lsb;
}
switch (Model.proto_opts[PROTOOPTS_FORMAT]) {
case FORMAT_REGULAR:
packet[12] = txid[2];
packet[13] = 0x0a;
break;
case FORMAT_X16_AH:
packet[12] = 0x00;
packet[13] = 0x00;
break;
case FORMAT_IRDRONE:
packet[12] = 0xe0;
packet[13] = 0x2e;
break;
}
packet[14] = checksum();
NRF24L01_WriteReg(NRF24L01_05_RF_CH,
bind ? bind_chan : rf_channels[rf_chan++]);
rf_chan %= sizeof(rf_channels);
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
NRF24L01_FlushTx();
XN297_WritePayload(packet, PACKET_SIZE);
NRF24L01_SetTxRxMode(TXRX_OFF);
NRF24L01_SetTxRxMode(TX_EN);
// 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));
if (telemetry) {
// switch radio to rx, no crc
NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x03);
}
// Check and adjust transmission power. We do this after
// transmission to not bother with timeout after power
// settings change - we have plenty of time until next
// packet.
if (tx_power != Model.tx_power) {
//Keep transmit power updated
tx_power = Model.tx_power;
NRF24L01_SetPower(tx_power);
}
#ifdef EMULATOR
dbgprintf("next chan 0x%02x, bind %d, data %02x",
bind ? bind_chan : rf_channels[rf_chan], bind,
packet[0]);
for (int i = 1; i < PACKET_SIZE; i++)
dbgprintf(" %02x", packet[i]);
dbgprintf("\n");
#endif
}
static void DM002_send_packet(u8 bind)
{
memcpy(&packet[5],(uint8_t *)"\x00\x7F\x7F\x7F\x00\x00\x00",7);
if(bind)
{
packet[0] = 0xAA;
packet[1] = rx_tx_addr[0];
packet[2] = rx_tx_addr[1];
packet[3] = rx_tx_addr[2];
packet[4] = rx_tx_addr[3];
}
else
{
packet[0]=0x55;
// Throttle : 0 .. 200
packet[1]=scale_channel(CHANNEL3,0,200);
// Other channels min 0x57, mid 0x7F, max 0xA7
packet[2] = scale_channel(CHANNEL4,0xA7,0x57); // rudder
packet[3] = scale_channel(CHANNEL1, 0xA7,0x57); // aileron
packet[4] = scale_channel(CHANNEL2, 0xA7, 0x57); // elevator
// Features
packet[9] = DM002_FLAG_HIGH // high rate
| GET_FLAG(CHANNEL_FLIP, DM002_FLAG_FLIP)
| GET_FLAG_INV(CHANNEL_LED, DM002_FLAG_LED)
| GET_FLAG(CHANNEL_CAMERA1, DM002_FLAG_CAMERA1)
| GET_FLAG(CHANNEL_CAMERA2, DM002_FLAG_CAMERA2)
| GET_FLAG(CHANNEL_HEADLESS,DM002_FLAG_HEADLESS)
| GET_FLAG(CHANNEL_RTH, DM002_FLAG_RTH);
// Packet counter
if(packet_count&0x03)
{
packet_count++;
hopping_frequency_no++;
hopping_frequency_no&=4;
}
packet_count&=0x0F;
packet[10] = packet_count;
packet_count++;
}
//CRC
for(uint8_t i=0;i<DM002_PACKET_SIZE-1;i++)
packet[11]+=packet[i];
// 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));
if (bind)
NRF24L01_WriteReg(NRF24L01_05_RF_CH, DM002_RF_BIND_CHANNEL);
else
NRF24L01_WriteReg(NRF24L01_05_RF_CH, hopping_frequency[hopping_frequency_no]);
// clear packet status bits and TX FIFO
NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);
NRF24L01_FlushTx();
XN297_WritePayload(packet, DM002_PACKET_SIZE);
if (tx_power != Model.tx_power) {
//Keep transmit power updated
tx_power = Model.tx_power;
NRF24L01_SetPower(tx_power);
}
}
