Rev 130 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + only for non-profit use
// + www.MikroKopter.com
// + see the File "License.txt" for further Informations
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include "main.h"
#include "uart.h"
unsigned char DebugGetAnforderung = 0,DebugDisplayAnforderung = 0,DebugDataAnforderung = 0,GetVersionAnforderung = 0;
unsigned volatile char SioTmp = 0;
unsigned volatile char SendeBuffer[MAX_SENDE_BUFF];
unsigned volatile char RxdBuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NMEABuffer[MAX_EMPFANGS_BUFF];
unsigned volatile char NeuerDatensatzEmpfangen = 0;
unsigned volatile char NeueKoordinateEmpfangen = 0;
unsigned volatile char UebertragungAbgeschlossen = 1;
unsigned volatile char CntCrcError = 0;
unsigned volatile char AnzahlEmpfangsBytes = 0;
unsigned volatile char PC_DebugTimeout = 0;
unsigned char PcZugriff = 100;
unsigned char MotorTest[4] = {0,0,0,0};
unsigned char MeineSlaveAdresse;
struct str_DebugOut DebugOut;
struct str_Debug DebugIn;
struct str_VersionInfo VersionInfo;
int Debug_Timer;
static uint8_t gpsState;
#define GPS_EMPTY 0
#define GPS_SYNC1 1
#define GPS_SYNC2 2
#define GPS_CLASS 3
#define GPS_LEN1 4
#define GPS_LEN2 5
#define GPS_FILLING 6
#define GPS_CKA 7
#define GPS_CKB 8
gpsInfo_t actualPos; // measured position (last gps record)
#define SYNC_CHAR1 0xb5
#define SYNC_CHAR2 0x62
#define CLASS_NAV 0x01
#define MSGID_STATUS 0x03
#define MSGID_POSUTM 0x08
#define MSGID_VELNED 0x12
typedef struct {
unsigned long ITOW; // time of week
uint8_t GPSfix; // GPSfix Type, range 0..6
uint8_t Flags; // Navigation Status Flags
uint8_t DiffS; // Differential Status
uint8_t res; // reserved
unsigned long TTFF; // Time to first fix (millisecond time tag)
unsigned long MSSS; // Milliseconds since Startup / Reset
uint8_t packetStatus;
} NAV_STATUS_t;
typedef struct {
unsigned long ITOW; // time of week
long EAST; // cm UTM Easting
long NORTH; // cm UTM Nording
long ALT; // cm altitude
uint8_t ZONE; // UTM zone number
uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
uint8_t packetStatus;
} NAV_POSUTM_t;
typedef struct {
unsigned long ITOW; // ms GPS Millisecond Time of Week
long VEL_N; // cm/s NED north velocity
long VEL_E; // cm/s NED east velocity
long VEL_D; // cm/s NED down velocity
unsigned long Speed; // cm/s Speed (3-D)
unsigned long GSpeed; // cm/s Ground Speed (2-D)
long Heading; // deg (1e-05) Heading 2-D
unsigned long SAcc; // cm/s Speed Accuracy Estimate
unsigned long CAcc; // deg Course / Heading Accuracy Estimate
uint8_t packetStatus;
} NAV_VELNED_t;
NAV_STATUS_t navStatus;
NAV_POSUTM_t navPosUtm;
NAV_VELNED_t navVelNed;
volatile char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered
volatile uint8_t CK_A, CK_B; // UBX checksum bytes
volatile unsigned short msgLen;
volatile uint8_t msgID;
volatile uint8_t ignorePacket; // true when previous packet was not processed
// distance to target position
long rollOffset; // in 10cm
long nickOffset;
#define GPS_INTCYCLES 100
#define GPS_I_LIMIT (long)(40 * MAINLOOPS_PER_SEC)
#ifdef GPS_DEBUG // if set then the GPS data is transfered to display
extern volatile uint8_t v24state;
char buf[200];
char *bp;
char *ep;
#endif
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Sende-Part der Datenübertragung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_TX)
{
static unsigned int ptr = 0;
unsigned char tmp_tx;
if(!UebertragungAbgeschlossen)
{
ptr++; // die [0] wurde schon gesendet
tmp_tx = SendeBuffer[ptr];
if((tmp_tx == '\r') || (ptr == MAX_SENDE_BUFF))
{
ptr = 0;
UebertragungAbgeschlossen = 1;
}
UDR = tmp_tx;
}
else ptr = 0;
}
void GPSscanData (void)
{
if (navStatus.packetStatus == 1) // valid packet
{
actualPos.state = navStatus.GPSfix;
navStatus.packetStatus = 0;
}
if (navPosUtm.packetStatus == 1) // valid packet
{
actualPos.northing = navPosUtm.NORTH; ///10;
actualPos.easting = navPosUtm.EAST; //10;
actualPos.altitude = navPosUtm.ALT; //10;
navPosUtm.packetStatus = 0;
}
if (navVelNed.packetStatus == 1){
actualPos.velNorth = navVelNed.VEL_N;
actualPos.velEast = navVelNed.VEL_E;
actualPos.velDown = navVelNed.VEL_D;
navVelNed.packetStatus = 0;}
if (actualPos.state == 2){ROT_ON; gps_new=1;} //-> Rot blinkt mit 4Hz wenn GPS 2D-Fix
else if (actualPos.state == 3){GRN_OFF; gps_new=1;} //-> Grün blinkt mit 4Hz wenn GPS 3D-Fix
}
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//++ Empfangs-Part der Datenübertragung, incl. CRC-Auswertung
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
SIGNAL(INT_VEC_RX)
{
static unsigned int crc;
static unsigned char crc1,crc2,buf_ptr;
static unsigned char UartState = 0;
unsigned char CrcOkay = 0;
SioTmp = UDR;
uint8_t c;
uint8_t re;
re = (UCSR0A & (_B1(FE0) | _B1(DOR0))); // any error occured ?
c = SioTmp;
#ifdef GPS_DEBUG
*bp++ = c;
if (bp >= (buf+200)) bp = buf;
if (v24state == 0)
{
v24state = 1;
UDR0 = *ep++;
if (ep >= buf+200)
ep = buf;
UCSR0B |= _B1(UDRIE0); //enable further irqs
}
#endif
if (re == 0)
{
switch (gpsState)
{
case GPS_EMPTY:
if (c == SYNC_CHAR1)
gpsState = GPS_SYNC1;
break;
case GPS_SYNC1:
if (c == SYNC_CHAR2)
gpsState = GPS_SYNC2;
else if (c != SYNC_CHAR1)
gpsState = GPS_EMPTY;
break;
case GPS_SYNC2:
if (c == CLASS_NAV)
gpsState = GPS_CLASS;
else
gpsState = GPS_EMPTY;
break;
case GPS_CLASS: // msg ID seen: init packed receive
msgID = c;
CK_A = CLASS_NAV + c;
CK_B = CLASS_NAV + CK_A;
gpsState = GPS_LEN1;
switch (msgID)
{
case MSGID_STATUS:
ubxP = (char*)&navStatus;
ubxEp = (char*)(&navStatus + sizeof(NAV_STATUS_t));
ubxSp = (char*)&navStatus.packetStatus;
ignorePacket = navStatus.packetStatus;
break;
case MSGID_POSUTM:
ubxP = (char*)&navPosUtm;
ubxEp = (char*)(&navPosUtm + sizeof(NAV_POSUTM_t));
ubxSp = (char*)&navPosUtm.packetStatus;
ignorePacket = navPosUtm.packetStatus;
break;
case MSGID_VELNED:
ubxP = (char*)&navVelNed;
ubxEp = (char*)(&navVelNed + sizeof(NAV_VELNED_t));
ubxSp = (char*)&navVelNed.packetStatus;
ignorePacket = navVelNed.packetStatus;
break;
default:
ignorePacket = 1;
ubxSp = (char*)0;
}
break;
case GPS_LEN1: // first len byte
msgLen = c;
CK_A += c;
CK_B += CK_A;
gpsState = GPS_LEN2;
break;
case GPS_LEN2: // second len byte
msgLen = msgLen + (c * 256);
CK_A += c;
CK_B += CK_A;
gpsState = GPS_FILLING; // next data will be stored in packet struct
break;
case GPS_FILLING:
CK_A += c;
CK_B += CK_A;
if ( !ignorePacket && ubxP < ubxEp)
*ubxP++ = c;
if (--msgLen == 0)
gpsState = GPS_CKA;
break;
case GPS_CKA:
if (c == CK_A)
gpsState = GPS_CKB;
else
gpsState = GPS_EMPTY;
break;
case GPS_CKB:
if (c == CK_B && ubxSp) // No error -> packet received successfully
*ubxSp = 1; // set packetStatus in struct
gpsState = GPS_EMPTY; // ready for next packet
break;
default:
gpsState = GPS_EMPTY; // ready for next packet
}
}
else // discard any data if error occured
{
gpsState = GPS_EMPTY;
GPSscanData (); //Test kann ggf. wieder gelöscht werden!
}
GPSscanData ();
if(buf_ptr >= MAX_EMPFANGS_BUFF) UartState = 0;
if(SioTmp == '\r' && UartState == 2)
{
UartState = 0;
crc -= RxdBuffer[buf_ptr-2];
crc -= RxdBuffer[buf_ptr-1];
crc %= 4096;
crc1 = '=' + crc / 64;
crc2 = '=' + crc % 64;
CrcOkay = 0;
if((crc1 == RxdBuffer[buf_ptr-2]) && (crc2 == RxdBuffer[buf_ptr-1])) CrcOkay = 1; else { CrcOkay = 0; CntCrcError++;};
if(!NeuerDatensatzEmpfangen && CrcOkay) // Datensatz schon verarbeitet
{
NeuerDatensatzEmpfangen = 1;
AnzahlEmpfangsBytes = buf_ptr;
RxdBuffer[buf_ptr] = '\r';
if(RxdBuffer[2] == 'R') wdt_enable(WDTO_250MS); // Reset-Commando
}
}
else
switch(UartState)
{
case 0:
if(SioTmp == '#' && !NeuerDatensatzEmpfangen) UartState = 1; // Startzeichen und Daten schon verarbeitet
buf_ptr = 0;
RxdBuffer[buf_ptr++] = SioTmp;
crc = SioTmp;
break;
case 1: // Adresse auswerten
UartState++;
RxdBuffer[buf_ptr++] = SioTmp;
crc += SioTmp;
break;
case 2: // Eingangsdaten sammeln
RxdBuffer[buf_ptr] = SioTmp;
if(buf_ptr < MAX_EMPFANGS_BUFF) buf_ptr++;
else UartState = 0;
crc += SioTmp;
break;
default:
UartState = 0;
break;
}
}
// --------------------------------------------------------------------------
void AddCRC(unsigned int wieviele)
{
unsigned int tmpCRC = 0,i;
for(i = 0; i < wieviele;i++)
{
tmpCRC += SendeBuffer[i];
}
tmpCRC %= 4096;
SendeBuffer[i++] = '=' + tmpCRC / 64;
SendeBuffer[i++] = '=' + tmpCRC % 64;
SendeBuffer[i++] = '\r';
UebertragungAbgeschlossen = 0;
UDR = SendeBuffer[0];
}
// --------------------------------------------------------------------------
void SendOutData(unsigned char cmd,unsigned char modul, unsigned char *snd, unsigned char len)
{
unsigned int pt = 0;
unsigned char a,b,c;
unsigned char ptr = 0;
SendeBuffer[pt++] = '#'; // Startzeichen
SendeBuffer[pt++] = modul; // Adresse (a=0; b=1,...)
SendeBuffer[pt++] = cmd; // Commando
while(len)
{
if(len) { a = snd[ptr++]; len--;} else a = 0;
if(len) { b = snd[ptr++]; len--;} else b = 0;
if(len) { c = snd[ptr++]; len--;} else c = 0;
SendeBuffer[pt++] = '=' + (a >> 2);
SendeBuffer[pt++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));
SendeBuffer[pt++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));
SendeBuffer[pt++] = '=' + ( c & 0x3f);
}
AddCRC(pt);
}
// --------------------------------------------------------------------------
void Decode64(unsigned char *ptrOut, unsigned char len, unsigned char ptrIn,unsigned char max) // Wohin mit den Daten; Wie lang; Wo im RxdBuffer
{
unsigned char a,b,c,d;
unsigned char ptr = 0;
unsigned char x,y,z;
while(len)
{
a = RxdBuffer[ptrIn++] - '=';
b = RxdBuffer[ptrIn++] - '=';
c = RxdBuffer[ptrIn++] - '=';
d = RxdBuffer[ptrIn++] - '=';
if(ptrIn > max - 2) break; // nicht mehr Daten verarbeiten, als empfangen wurden
x = (a << 2) | (b >> 4);
y = ((b & 0x0f) << 4) | (c >> 2);
z = ((c & 0x03) << 6) | d;
if(len--) ptrOut[ptr++] = x; else break;
if(len--) ptrOut[ptr++] = y; else break;
if(len--) ptrOut[ptr++] = z; else break;
}
}
// --------------------------------------------------------------------------
void BearbeiteRxDaten(void)
{
if(!NeuerDatensatzEmpfangen) return;
unsigned int tmp_int_arr1[1];
unsigned int tmp_int_arr2[2];
unsigned int tmp_int_arr3[3];
unsigned char tmp_char_arr2[2];
unsigned char tmp_char_arr3[3];
unsigned char tmp_char_arr4[4];
//if(!MotorenEin)
PcZugriff = 255;
switch(RxdBuffer[2])
{
case 'c':// Debugdaten incl. Externe IOs usw
Decode64((unsigned char *) &DebugIn,sizeof(DebugIn),3,AnzahlEmpfangsBytes);
/* for(unsigned char i=0; i<4;i++)
{
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2], DebugIn.Analog[i]);
EE_CheckAndWrite(&EE_Buffer[EE_DEBUGWERTE + i*2 + 1], DebugIn.Analog[i] >> 8);
}*/
//RemoteTasten |= DebugIn.RemoteTasten;
DebugDataAnforderung = 1;
break;
case 'h':// x-1 Displayzeilen
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
RemoteTasten |= tmp_char_arr2[0];
DebugDisplayAnforderung = 1;
break;
case 't':// Motortest
Decode64((unsigned char *) &MotorTest[0],sizeof(MotorTest),3,AnzahlEmpfangsBytes);
break;
case 'v': // Version-Anforderung und Ausbaustufe
GetVersionAnforderung = 1;
break;
case 'g':// "Get"-Anforderung für Debug-Daten
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
DebugGetAnforderung = 1;
break;
case 'q':// "Get"-Anforderung für Settings
// Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
Decode64((unsigned char *) &tmp_char_arr2[0],sizeof(tmp_char_arr2),3,AnzahlEmpfangsBytes);
if(tmp_char_arr2[0] != 0xff)
{
if(tmp_char_arr2[0] > 5) tmp_char_arr2[0] = 5;
ReadParameterSet(tmp_char_arr2[0], (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
SendOutData('L' + tmp_char_arr2[0] -1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
}
else
SendOutData('L' + GetActiveParamSetNumber()-1,MeineSlaveAdresse,(unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE);
break;
case 'l':
case 'm':
case 'n':
case 'o':
case 'p': // Parametersatz speichern
Decode64((unsigned char *) &EE_Parameter.Kanalbelegung[0],STRUCT_PARAM_LAENGE,3,AnzahlEmpfangsBytes);
WriteParameterSet(RxdBuffer[2] - 'l' + 1, (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], RxdBuffer[2] - 'l' + 1); // aktiven Datensatz merken
Piep(GetActiveParamSetNumber());
break;
}
// DebugOut.AnzahlZyklen = Debug_Timer_Intervall;
NeuerDatensatzEmpfangen = 0;
}
//############################################################################
//Routine für die Serielle Ausgabe
int uart_putchar (char c)
//############################################################################
{
if (c == '\n')
uart_putchar('\r');
//Warten solange bis Zeichen gesendet wurde
loop_until_bit_is_set(USR, UDRE);
//Ausgabe des Zeichens
UDR = c;
return (0);
}
// --------------------------------------------------------------------------
void WriteProgramData(unsigned int pos, unsigned char wert)
{
//if (ProgramLocation == IN_RAM) Buffer[pos] = wert;
// else eeprom_write_byte(&EE_Buffer[pos], wert);
// Buffer[pos] = wert;
}
//############################################################################
//INstallation der Seriellen Schnittstelle
void UART_Init (void)
//############################################################################
{
//Enable TXEN im Register UCR TX-Data Enable & RX Enable
UCR=(1 << TXEN) | (1 << RXEN);
// UART Double Speed (U2X)
USR |= (1<<U2X);
// RX-Interrupt Freigabe
UCSRB |= (1<<RXCIE);
// TX-Interrupt Freigabe
UCSRB |= (1<<TXCIE);
//Teiler wird gesetzt
UBRR=(SYSCLK / (BAUD_RATE * 8L) - 1);
//UBRR = 33;
//öffnet einen Kanal für printf (STDOUT)
//fdevopen (uart_putchar, 0);
//sbi(PORTD,4);
Debug_Timer = SetDelay(200);
}
//---------------------------------------------------------------------------------------------
void DatenUebertragung(void)
{
static char dis_zeile = 0;
if(!UebertragungAbgeschlossen) return;
if(DebugGetAnforderung && UebertragungAbgeschlossen) // Bei Get werden die vom PC einstellbaren Werte vom PC zurückgelesen
{
SendOutData('G',MeineSlaveAdresse,(unsigned char *) &DebugIn,sizeof(DebugIn));
DebugGetAnforderung = 0;
}
if((CheckDelay(Debug_Timer) || DebugDataAnforderung) && UebertragungAbgeschlossen)
{
SendOutData('D',MeineSlaveAdresse,(unsigned char *) &DebugOut,sizeof(DebugOut));
DebugDataAnforderung = 0;
Debug_Timer = SetDelay(MIN_DEBUG_INTERVALL);
}
if(DebugDisplayAnforderung && UebertragungAbgeschlossen)
{
Menu();
DebugDisplayAnforderung = 0;
if(++dis_zeile == 4)
{
SendOutData('4',0,&PPM_in,sizeof(PPM_in)); // DisplayZeile übertragen
dis_zeile = -1;
}
else SendOutData('0' + dis_zeile,0,&DisplayBuff[20 * dis_zeile],20); // DisplayZeile übertragen
}
if(GetVersionAnforderung && UebertragungAbgeschlossen)
{
SendOutData('V',MeineSlaveAdresse,(unsigned char *) &VersionInfo,sizeof(VersionInfo));
GetVersionAnforderung = 0;
}
}