Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 780 → Rev 781

/branches/V0.68d Code Redesign killagreg/GPS.c
19,15 → 19,14
int16_t GPS_Roll = 0;
 
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 
 
 
typedef struct
{
int32_t Northing;
int32_t Easting;
int32_t Longitude;
int32_t Latitude;
uint8_t Status;
 
} GPS_Pos_t;
 
// GPS coordinates for hold position
38,15 → 37,37
 
// ---------------------------------------------------------------------------------
 
// set home position to current hold positon
// set home position to current positon
void GPS_SetHomePosition(void)
{
HomePosition.Northing = HoldPosition.Northing;
HomePosition.Easting = HoldPosition.Easting;
HomePosition.Status = HoldPosition.Status;
if(HomePosition.Status == VALID) BeepTime = 1000; // signal if new home position was set
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
{
HomePosition.Longitude = GPSInfo.longitude;
HomePosition.Latitude = GPSInfo.latitude;
HomePosition.Status = VALID;
BeepTime = 1000; // signal if new home position was set
}
else
{
HomePosition.Status = INVALID;
}
}
 
// set hold position to current positon
void GPS_SetHoldPosition(void)
{
if( ((GPSInfo.status == VALID) || (GPSInfo.status == PROCESSED)) && GPSInfo.satfix == SATFIX_3D)
{
HoldPosition.Longitude = GPSInfo.longitude;
HoldPosition.Latitude = GPSInfo.latitude;
HoldPosition.Status = VALID;
}
else
{
HoldPosition.Status = INVALID;
}
}
 
// clear home position
void GPS_ClearHomePosition(void)
{
64,13 → 85,27
void GPS_PDController(GPS_Pos_t *TargetPos)
{
int32_t coscompass, sincompass;
int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
int32_t PD_North = 0, PD_East = 0;
static int32_t cos_target_latitude = 1;
 
if( (TargetPos->Status == VALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
 
if( (TargetPos->Status != INVALID) && (GPSInfo.status == VALID) && (GPSInfo.satfix == SATFIX_3D))
{
GPSPosDev_North = GPSInfo.utmnorth - TargetPos->Northing;
GPSPosDev_East = GPSInfo.utmeast - TargetPos->Easting;
// calculate position deviation from latitude and longitude differences
GPSPosDev_North = (GPSInfo.latitude - TargetPos->Latitude); // to calculate real cm we would need *111/100 additionally
GPSPosDev_East = (GPSInfo.longitude - TargetPos->Longitude); // to calculate real cm we would need *111/100 additionally
// recalculate the target latitude projection only if the target data are updated
// to save time
if (TargetPos->Status != PROCESSED)
{
cos_target_latitude = (int32_t)c_cos_8192((int16_t)(TargetPos->Latitude/10000000L));
TargetPos->Status = PROCESSED;
}
// calculate latitude projection
GPSPosDev_East *= cos_target_latitude;
GPSPosDev_East /= 8192;
 
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
158,7 → 193,7
BeepTime = 100; // beep if signal is neccesary
}
break;
case PROCESSED: // if gps data are already processed
case PROCESSED: // if gps data are already processed do nothing
// downcount timeout
if(GPSTimeout) GPSTimeout--;
// if no new data arrived within timeout set current data invalid
170,21 → 205,19
}
break;
case VALID: // new valid data from gps device
// if the gps data quality is sufficient
// if the gps data quality is good
if (GPSInfo.satfix == SATFIX_3D)
{
switch(GPS_Task) // check what's to do
{
case TSK_IDLE:
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
// update hold position to current gps position
GPS_SetHoldPosition(); // can get invalid if gps signal is bad
// disable gps control
GPS_Neutral();
break; // eof TSK_IDLE
case TSK_HOLD:
if(HoldPosition.Status == VALID)
if(HoldPosition.Status != INVALID)
{
// if sticks are centered (no manual control)
if ((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
194,22 → 227,23
else // MK controlled by user
{
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = GPSInfo.status;
GPS_SetHoldPosition();
// disable gps control
GPS_Neutral();
}
}
else // invalid Hold Position
{ // try to catch a valid hold position from gps data input
GPS_SetHoldPosition();
GPS_Neutral();
}
break; // eof TSK_HOLD
case TSK_HOME:
if(HomePosition.Status == VALID)
if(HomePosition.Status != INVALID)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = GPSInfo.status;
GPS_SetHoldPosition();
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
224,13 → 258,22
{
BeepTime = 50; // signal invalid home position
// try to hold at least the position as a fallback option
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE) && (HoldPosition.Status == VALID))
 
if (HoldPosition.Status != INVALID)
{
GPS_PDController(&HoldPosition);
// if sticks are centered (no manual control)
if((MaxStickPitch < GPS_STICK_SENSE) && (MaxStickRoll < GPS_STICK_SENSE))
{
GPS_PDController(&HoldPosition);
}
else // manual control or no reference position
{
GPS_Neutral();
}
}
else // manual control or no rteference position
{
else
{ // try to catch a valid hold position
GPS_SetHoldPosition();
GPS_Neutral();
}
}
240,7 → 283,6
break; // eof default
} // eof switch GPS_Task
} // eof 3D-FIX
 
else // no 3D-SATFIX
{ // disable gps control
GPS_Neutral();
/branches/V0.68d Code Redesign killagreg/makefile
13,6 → 13,7
#OPTIONS
COMPASS = MM3
#COMPASS = CMPS03
#EXT = WALTER
#-------------------------------------------------------------------
 
ifeq ($(MCU), atmega644)
140,9 → 141,13
ifeq ($(COMPASS), CMPS03)
CFLAGS += -DUSE_CMPS03
endif
ifeq ($(EXT), WALTER)
CFLAGS += -DUSE_WALTER_EXT
endif
 
 
 
 
# Optional assembler flags.
# -Wa,...: tell GCC to pass this to the assembler.
# -ahlms: create listing
/branches/V0.68d Code Redesign killagreg/mm3.c
81,10 → 81,19
// set MISO (PB6) as input
DDRB &= ~(1<<DDB6);
 
#ifdef USE_WALTER_EXT // walthers board
// Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET
DDRB |= (1<<DDB2);
DDRC |= (1<<DDC6);
// set pins permanent to low
PORTB &= ~((1<<PORTB2));
PORTC &= ~((1<<PORTC6));
#else // killagregs board
// Output Pins PC4->MM3_SS ,PC5->MM3_RESET
DDRC |= (1<<DDC4)|(1<<DDC5);
// set pins permanent to low
PORTC &= ~((1<<PORTC4)|(1<<PORTC5));
#endif
 
// Initialize SPI-Interface
// Enable interrupt (SPIE=1)
91,9 → 100,9
// Enable SPI bus (SPE=1)
// MSB transmitted first (DORD = 0)
// Master SPI Mode (MSTR=1)
// Clock polarity low whn idle (CPOL=0)
// clock phase sample at leading edge (CPHA=0)
// clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
// Clock polarity low when idle (CPOL=0)
// Clock phase sample at leading edge (CPHA=0)
// Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz
SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0);
SPSR &= ~(1<<SPI2X);
 
123,14 → 132,22
switch (MM3.STATE)
{
case MM3_STATE_RESET:
#ifdef USE_WALTER_EXT // walthers board
PORTC &= ~(1<<PORTC6); // select slave
PORTB |= (1<<PORTB2); // PB2 to High, MM3 Reset
#else
PORTC &= ~(1<<PORTC4); // select slave
PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset
#endif
MM3.STATE = MM3_STATE_START_TRANSFER;
return;
 
case MM3_STATE_START_TRANSFER:
#ifdef USE_WALTER_EXT // walthers board
PORTB &= ~(1<<PORTB2); // PB2 auf Low (was 102.4 µs at high level)
#else
PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level)
 
#endif
// write to SPDR triggers automatically the transfer MOSI MISO
// MM3 Period, + AXIS code
switch(MM3.AXIS)
211,7 → 228,11
break;
}
}
#ifdef USE_WALTER_EXT // walthers board
PORTC |= (1<<PORTC6); // deselect slave
#else
PORTC |= (1<<PORTC4); // deselect slave
#endif
MM3.STATE = MM3_STATE_RESET;
// Update timeout is called every 102.4 µs.
// It takes 2 cycles to write a measurement data request for one axis and
/branches/V0.68d Code Redesign killagreg/ubx.c
21,7 → 21,6
 
#define UBX_ID_POSLLH 0x02
#define UBX_ID_SOL 0x06
#define UBX_ID_POSUTM 0x08
#define UBX_ID_VELNED 0x12
 
#define UBX_SYNC1_CHAR 0xB5
60,16 → 59,6
} GPS_POSLLH_t;
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t EAST; // cm UTM Easting
int32_t NORTH; // cm UTM Nording
int32_t ALT; // cm altitude
uint8_t ZONE; // UTM zone number
uint8_t HEM; // Hemisphere Indicator (0=North, 1=South)
uint8_t Status;
} GPS_POSUTM_t;
 
typedef struct {
uint32_t ITOW; // ms GPS Millisecond Time of Week
int32_t VEL_N; // cm/s NED north velocity
int32_t VEL_E; // cm/s NED east velocity
84,7 → 73,6
 
GPS_SOL_t GpsSol = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, INVALID};
GPS_POSLLH_t GpsPosLlh = {0,0,0,0,0,0,0, INVALID};
GPS_POSUTM_t GpsPosUtm = {0,0,0,0,0,0, INVALID};
GPS_VELNED_t GpsVelNed = {0,0,0,0,0,0,0,0,0, INVALID};
GPS_INFO_t GPSInfo = {0,0,0,0,0,0,0,0,0,0, INVALID};
 
107,13 → 95,6
GPSInfo.altitude = GpsPosLlh.HEIGHT;
GpsPosLlh.Status = PROCESSED; // never update old data
}
if (GpsPosUtm.Status == VALID) // valid packet
{
GPSInfo.utmeast = GpsPosUtm.EAST;
GPSInfo.utmnorth = GpsPosUtm.NORTH;
GPSInfo.utmalt = GpsPosUtm.ALT;
GpsPosUtm.Status = PROCESSED; // never update old data
}
if (GpsVelNed.Status == VALID) // valid packet
{
GPSInfo.veleast = GpsVelNed.VEL_E;
121,7 → 102,7
GPSInfo.veltop = -GpsVelNed.VEL_D;
GpsVelNed.Status = PROCESSED; // never update old data
}
if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsPosUtm.Status != INVALID) && (GpsVelNed.Status != INVALID))
if ((GpsSol.Status != INVALID) && (GpsPosLlh.Status != INVALID) && (GpsVelNed.Status != INVALID))
{
GPSInfo.status = VALID; // set valid if data are updated
}
156,12 → 137,6
case UBXSTATE_CLASS: // check message identifier
switch(c)
{
case UBX_ID_POSUTM: // utm position
ubxP = (int8_t *)&GpsPosUtm; // data start pointer
ubxEp = (int8_t *)(&GpsPosUtm + sizeof(GPS_POSUTM_t)); // data end pointer
ubxSp = (int8_t *)&GpsPosUtm.Status; // status pointer
break;
 
case UBX_ID_POSLLH: // geodetic position
ubxP = (int8_t *)&GpsPosLlh; // data start pointer
ubxEp = (int8_t *)(&GpsPosLlh + sizeof(GPS_POSLLH_t)); // data end pointer
/branches/V0.68d Code Redesign killagreg/ubx.h
19,26 → 19,21
/* enable the UBX protocol at the gps receiver with the following messages enabled
01-02 NAV - POSLLH
01-06 Nav - SOL
01-08 NAV - POSUTM
01-12 NAV - VELNED */
 
 
typedef struct
{
uint8_t status; // status of data: invalid | valid
uint8_t satnum; // number of satelites
uint8_t satfix; // type of satfix
int32_t utmnorth; // in cm (+ = north)
int32_t utmeast; // in cm (+ = east)
int32_t utmalt; // in cm (+ = top)
int32_t longitude; // in 1e-07 deg
int32_t latitude; // in 1e-07 deg
int32_t altitude; // in mm
uint32_t PAcc; // in cm 3d position accuracy
int32_t velnorth; // in cm/s
int32_t veleast; // in cm/s
int32_t veltop; // in cm/s
uint32_t VAcc; // in cm/s 3d velocity accuracy
int32_t longitude; // in 1e-07 deg
int32_t latitude; // in 1e-07 deg
int32_t altitude; // in mm
uint8_t status; // status of data: invalid | valid
} GPS_INFO_t;
 
//here you will find the current gps info