Rev 1199 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#include "dsl.h"
#include "rc.h"
/*
1. GND (at channel 4 side)
2. TxD
3. RxD
4. 5V
*/
// dsl protocol parser state machine
#define DSLSTATE_SYNC1 0
#define DSLSTATE_SYNC2 1
#define DSLSTATE_HDR 2
#define DSLSTATE_DATA 3
#define DSLSTATE_CRC 4
#define DSL_SYNC1_CHAR 0xFF
#define DSL_SYNC2_CHAR 0xFF
uint8_t dsl_RSSI = 0;
uint8_t dsl_Battery = 0;
uint8_t dsl_Allocation = 0;
uint8_t PacketBuffer[6];
typedef union
{
int16_t Servo[2];
uint8_t byte[4];
} ServoPair_t;
ServoPair_t ServoPair;
// This function is called, when a new servo signal is properly received.
// Parameters: servo - servo number (0-9)
// signal - servo signal between 7373 (1ms) and 14745 (2ms)
void dsl_new_signal(uint8_t channel, int16_t signal)
{
int16_t tmp;
uint8_t index = channel + 1; // mk channels start with 1
// Signal vom ACTDSL-Empfänger liegt zwischen
// 7373 (1ms) und 14745 (2ms).
signal-= 11059; // shift to neutral
signal/= 24; // scale to mk rc resolution
// calculate exponential history for signal
tmp = (3 * (PPM_in[index]) + signal) / 4;
if(tmp > signal+1) tmp--; else
if(tmp < signal-1) tmp++;
// calculate signal difference on good signal level
if(RC_Quality >= 195) PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for noise reduction
else PPM_diff[index] = 0;
PPM_in[index] = tmp; // update channel value
if(index == 4) NewPpmData = 0;
}
// This function is called within dsl_parser(), when a complete
// data packet with valid checksum has been received.
void dsl_decode_packet(void)
{
uint8_t i;
if(PacketBuffer[0] == 0x1F) // separate status frame
{
dsl_Allocation = PacketBuffer[1]; // Get frequency allocation
// ?? = PacketBuffer[2];
dsl_RSSI = PacketBuffer[3]; // Get signal quality
RC_Quality = dsl_RSSI; // have to be scaled
dsl_Battery = PacketBuffer[4]; // Get voltage of battery supply
// ?? = PacketBuffer[5];
}
else // analyze servo pair frame
{
i = PacketBuffer[0] & 0x0F; // last 4 bits of the header indicates servo pair
if(i < 10)
{
// big to little endian
ServoPair.byte[1] = PacketBuffer[1];
ServoPair.byte[0] = PacketBuffer[2];
ServoPair.byte[3] = PacketBuffer[3];
ServoPair.byte[2] = PacketBuffer[4];
dsl_new_signal(i, ServoPair.Servo[0]);
dsl_new_signal(i+1,ServoPair.Servo[0]);
}
}
}
// Status Frame: |0xFF|0xFF|0x1F|FREQALLOC|??|RSSI|VBAT|??|CRC|
// FREQ ALLOC = 35, 40, 72
// RSSI = 0.. 255 // Received signal strength indicator
// VBAT = 0...255 // supply voltage (0.0V.. 8.2V)
//
// Servo Pair: |0xFF|0xFF|0x1X|D0|D1|D2|D3|CRC|
// X is channel index of 1 servo value
// D0, D1 is servo channel X
// D2, D3 is servo channel X+1
// this function should be called within the UART RX ISR
void dsl_parser(uint8_t c)
{
static uint8_t crc = 0;
static uint8_t cnt = 0;
static uint8_t packet_len = 0;
static uint8_t dslstate = DSLSTATE_SYNC1;
switch(dslstate)
{
case DSLSTATE_SYNC1: // check 1st sync byte
if (c == DSL_SYNC1_CHAR) dslstate = DSLSTATE_SYNC2;
else dslstate = DSLSTATE_SYNC1; // out of synchronization
break;
case DSLSTATE_SYNC2: // check 2nd sync byte
if (c == DSL_SYNC2_CHAR) dslstate = DSLSTATE_HDR; // snchrinization complete trigger to cmd
else dslstate = DSLSTATE_SYNC1; // out of synchronization
break;
case DSLSTATE_HDR: // check header byte
if ((c & 0xF0) != 0x10)
{
dslstate = DSLSTATE_SYNC1;
}
else
{
cnt = 0;
PacketBuffer[cnt++] = c;
crc = c;
if(c == 0x1F) packet_len = 5;
else packet_len = 4;
dslstate = DSLSTATE_DATA;
}
break;
case DSLSTATE_DATA:
PacketBuffer[cnt++] = c;
crc += c;
if(cnt > packet_len) dslstate = DSLSTATE_CRC;
break;
case DSLSTATE_CRC:
// Calculate checksum
crc = ~crc;
if (crc == 0xFF) crc = 0xFE;
// if crc matches
if (c == crc) dsl_decode_packet();
dslstate = DSLSTATE_SYNC1;
break;
default: // unknown ubx state
dslstate = DSLSTATE_SYNC1;
break;
}
}