WebSVN - FlightCtrl - Rev 894 - /branches/KalmanFilter MikeW/gps.c

/*
Copyright 2008, by Michael Walter

All functions written by Michael Walter are free software and can be redistributed and/or modified under the terms of the GNU Lesser
General Public License as published by the Free Software Foundation. This program 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 Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public
License along with this program. If not, see <http://www.gnu.org/licenses/>.

Please note: The software is based on the framework provided by H. Buss and I. Busker in their Mikrokopter projekt. All functions that
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
unless it is stated otherwise.
*/

#include "main.h"
#include "kafi.h"

#include "mymath.h"
#include <math.h>

int GPSTracking = 0;
int targetPosValid = 0;
int homePosValid = 0;

uint8_t gpsState;

gpsInfo_t actualPos; // measured position (last gps record)
gpsInfo_t targetPos; // measured position (last gps record)
gpsInfo_t homePos; // measured position (last gps record)

NAV_STATUS_t navStatus;
NAV_POSLLH_t navPosLlh;
NAV_POSUTM_t navPosUtm;
NAV_VELNED_t navVelNed;

signed int GPS_Nick = 0;
signed int GPS_Roll = 0;

long distanceNS = 0;
long distanceEW = 0;
long velocityNS = 0;
long velocityEW = 0;
unsigned long maxDistance = 0;

int roll_gain = 0;
int nick_gain = 0;
int nick_gain_p = 0;
int nick_gain_d = 0;
int roll_gain_p = 0;
int roll_gain_d = 0;
int Override = 0;
int TargetGier = 0;

extern int sollGier;

char *ubxP, *ubxEp, *ubxSp; // pointers to packet currently transfered
uint8_t CK_A, CK_B; // UBX checksum bytes
uint8_t ignorePacket; // true when previous packet was not processed
unsigned short msgLen;
uint8_t msgID;

void GPSscanData (void);
void GPSupdate(void);

/* ****************************************************************************
Functionname: GPSupdate */ /*!
Description:

@return void
@pre -
@post -
@author Michael Walter
**************************************************************************** */
void GPSupdate(void)
{
float SIN_H, COS_H;
long max_p = 0;
long max_d = 0;

if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]])> 15 ||
abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]])> 10 ||
abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]])> 15)
{
Override = 1;
}
else
{
Override = 0;
}

/* Set Home Position */
if ((actualPos.state > 2) &&
(homePosValid == 0))
{
/* Save Current Position */
homePos.north = actualPos.north;
homePos.east = actualPos.east;
homePos.altitude = actualPos.altitude ;
homePosValid = 1;
}

/* Set Target Position */
if ((actualPos.state > 2) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < -100))
{
/* Save Current Position */
targetPos.north = actualPos.north;
targetPos.east = actualPos.east;
targetPos.altitude = actualPos.altitude ;
targetPosValid = 1;

if(BeepMuster == 0xffff)
{
beeptime = 5000;
BeepMuster = 0x0100;
}
}

if ((GPSTracking == 1) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] < 100))
{
GPSTracking = 0;
beeptime = 5000;
BeepMuster = 0x0080;
}

/* The System is in Tracking State*/
if ((GPSTracking == 0) &&
(targetPosValid == 1) &&
(PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] > 100) &&
(actualPos.state > 2))
{
GPSTracking = 1;
beeptime = 5000;
BeepMuster = 0x0c00;
}

max_p = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 170)) / 20);
max_d = (MAX(0, (PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 170)) / 40);

#if 0
DebugOut.Analog[11] = max_p;
DebugOut.Analog[12] = max_d;
#endif

static int GPSTrackingCycles = 0;
long maxGainPos = 140;
long maxGainVel = 140;

/* Slowly ramp up gain */
if (GPSTrackingCycles < 1000)
{
GPSTrackingCycles++;
}
maxGainPos = (maxGainPos * GPSTrackingCycles) / 1000;
maxGainVel = (maxGainVel * GPSTrackingCycles) / 1000;

if (actualPos.state > 2 )
{
distanceNS = actualPos.north - targetPos.north; // in 0.1m
distanceEW = actualPos.east - targetPos.east; // in 0.1m
velocityNS = actualPos.velNorth; // in 0.1m/s
velocityEW = actualPos.velEast; // in 0.1m/s
maxDistance = sqrt(distanceNS * distanceNS + distanceEW * distanceEW);

// Limit GPS_Nick to 25
nick_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceNS* max_p)/50))); //m
nick_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velNorth * max_d)/50))); //m/s
roll_gain_p = (long) MAX(-maxGainPos, MIN(maxGainPos, ((distanceEW * max_p)/50))); //m
roll_gain_d = (long) MAX(-maxGainVel, MIN(maxGainVel, ((actualPos.velEast * max_d)/50))); //m/s

TargetGier = c_atan2(-distanceEW / 8, -distanceNS / 8) * 10;
}

if ((GPSTracking == 1) &&
(actualPos.state > 2) &&
(Override == 0))
{
/* Calculate Distance to Target */
SIN_H = (float) sin(status.Psi);
COS_H =(float) cos(status.Psi);

/* PD-Regler */
nick_gain = nick_gain_p + nick_gain_d;
roll_gain = -(roll_gain_p + roll_gain_d);

GPS_Nick = (int) (COS_H * (float) nick_gain - SIN_H * (float) roll_gain);
GPS_Roll = (int) (SIN_H * (float) nick_gain + COS_H * (float) roll_gain);

GPS_Nick = MAX(-maxGainPos, MIN(maxGainPos, GPS_Nick));
GPS_Roll = MAX(-maxGainPos, MIN(maxGainPos, GPS_Roll));

/* Turn the mikrokopter slowly towards the target position */
{
int OffsetGier;
if (maxDistance / 10 > 2)
{
OffsetGier = 2;
}
else
{
OffsetGier = 0;
}

if (TargetGier < 0)
{
TargetGier += 3600;
}
if (TargetGier > sollGier)
{
if (TargetGier - sollGier < 1800)
{
sollGier += OffsetGier;
}
else
{
sollGier -= OffsetGier;
}
}
else
{
if (sollGier - TargetGier < 1800)
{
sollGier -= OffsetGier;
}
else
{
sollGier += OffsetGier;
}
}
}
#if 0
/* Go round in a square */
if (maxDistance / 10 < 10)
{
static int iState = 0;
switch (iState)
{
case 0:
targetPos.north += 400;
targetPos.east += 0;
GPSTrackingCycles = 0;
iState++;
break;
case 1:
targetPos.north += 0;
targetPos.east += 400;
GPSTrackingCycles = 0;
iState++;
break;
case 2:
targetPos.north -= 400;
targetPos.east += 0;
GPSTrackingCycles = 0;
iState++;
break;
case 3:
targetPos.north += 0;
targetPos.east -= 400;
GPSTrackingCycles = 0;
iState=0;
break;
default:
iState=0;
break;
}
beeptime = 5000;
BeepMuster = 0x0c00;
}
#endif
}
else
{
GPS_Nick = 0;
GPS_Roll = 0;
GPSTrackingCycles = 0;
}
}

/* ****************************************************************************
Functionname: InitGPS */ /*!
Description:

@return void
@pre -
@post -
@author
**************************************************************************** */
void InitGPS(void)
{
// USART1 Control and Status Register A, B, C and baud rate register
uint8_t sreg = SREG;
//uint16_t ubrr = (uint16_t) ((uint32_t) SYSCLK/(8 * USART1_BAUD) - 1);

// disable all interrupts before reconfiguration
cli();

// disable RX-Interrupt
UCSR1B &= ~(1 << RXCIE1);
// disable TX-Interrupt
UCSR1B &= ~(1 << TXCIE1);
// disable DRE-Interrupt
UCSR1B &= ~(1 << UDRIE1);

// set direction of RXD1 and TXD1 pins
// set RXD1 (PD2) as an input pin
PORTD |= (1 << PORTD2);
DDRD &= ~(1 << DDD2);

// set TXD1 (PD3) as an output pin
PORTD |= (1 << PORTD3);
DDRD |= (1 << DDD3);

// USART0 Baud Rate Register
// set clock divider
UBRR1H = 0;
UBRR1L = BAUDRATE;

// enable double speed operation
//UCSR1A |= (1 << U2X1);
UCSR1A = _B0(U2X1);

// enable receiver and transmitter
UCSR1B = (1 << TXEN1) | (1 << RXEN1);
// set asynchronous mode
UCSR1C &= ~(1 << UMSEL11);
UCSR1C &= ~(1 << UMSEL10);
// no parity
UCSR1C &= ~(1 << UPM11);
UCSR1C &= ~(1 << UPM10);
// 1 stop bit
UCSR1C &= ~(1 << USBS1);
// 8-bit
UCSR1B &= ~(1 << UCSZ12);
UCSR1C |= (1 << UCSZ11);
UCSR1C |= (1 << UCSZ10);

// flush receive buffer explicit
while ( UCSR1A & (1<<RXC1) ) UDR1;

// enable interrupts at the end
// enable RX-Interrupt
UCSR1B |= (1 << RXCIE1);
// enable TX-Interrupt
UCSR1B |= (1 << TXCIE1);
// enable DRE interrupt
//UCSR1B |= (1 << UDRIE1);

// restore global interrupt flags
SREG = sreg;

gpsState = GPS_EMPTY;
}

/* ****************************************************************************
Functionname: InitGPS */ /*!
Description: Copy GPS paket data to actualPos

@return void
@pre -
@post -
@author
**************************************************************************** */
void GPSscanData (void)
{
if (navStatus.packetStatus == 1) // valid packet
{
actualPos.state = navStatus.GPSfix;
if ((actualPos.state > 1) && (GPSTracking == 0))
{
GRN_FLASH;
}
navStatus.packetStatus = 0;
}

if (navPosUtm.packetStatus == 1) // valid packet
{
actualPos.north = navPosUtm.NORTH/10L;
actualPos.east = navPosUtm.EAST/10L;
actualPos.altitude = navPosUtm.ALT/100;
actualPos.ITOW = navPosUtm.ITOW;
navPosUtm.packetStatus = 0;
}
/*
if (navPosLlh.packetStatus == 1)
{
actualPos.longi = navPosLlh.LON;
actualPos.lati = navPosLlh.LAT;
actualPos.height = navPosLlh.HEIGHT;
navPosLlh.packetStatus = 0;
}
*/
if (navVelNed.packetStatus == 1)
{
actualPos.velNorth = navVelNed.VEL_N;
actualPos.velEast = navVelNed.VEL_E;
actualPos.velDown = navVelNed.VEL_D;
actualPos.groundSpeed = navVelNed.GSpeed;
navVelNed.packetStatus = 0;
}
}

/* ****************************************************************************
Functionname: SIGNAL */ /*!
Description:

@return void
@pre -
@post -
@author
**************************************************************************** */
SIGNAL (SIG_USART1_RECV)
{
uint8_t c;
uint8_t re;

re = (UCSR1A & (_B1(FE1) | _B1(DOR1))); // any error occured ?
c = UDR1;

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_POSLLH:
ubxP = (char*)&navPosLlh;
ubxEp = (char*)(&navPosLlh + sizeof(NAV_POSLLH_t));
ubxSp = (char*)&navPosLlh.packetStatus;
ignorePacket = navPosLlh.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
GPSscanData();
}
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;
}
}

Subversion Repositories FlightCtrl

(root)/branches/KalmanFilter MikeW/gps.c - Rev 894