Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 745 → Rev 746

/branches/V0.68d Code Redesign killagreg/GPS.c
14,9 → 14,10
int16_t GPS_Pitch = 0;
int16_t GPS_Roll = 0;
 
int32_t GPS_P_Factor = 0;
int32_t GPS_D_Factor = 0;
int32_t GPS_P_Factor = 0, GPS_D_Factor = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
 
 
typedef struct
{
int32_t Northing;
33,13 → 34,78
 
// ---------------------------------------------------------------------------------
 
// set current hold position a home 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
}
 
void GPS_Main(void)
 
// disable GPS contrl sticks
void GPS_Neutral(void)
{
GPS_Pitch = 0;
GPS_Roll = 0;
}
 
// calculates the GPS control sticks salues from the position deviation
void GPS_PDController(void)
{
int32_t coscompass, sincompass;
int32_t P_North = 0, D_North = 0, P_East = 0, D_East = 0;
int32_t PD_North = 0, PD_East = 0;
int32_t GPSPosDev_North = 0, GPSPosDev_East = 0;
//Calculate PD-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
 
// GPS to pitch and roll settings
 
//A positive pitch angle moves head downwards (flying forward).
//A positive roll angle tilts left side downwards (flying left).
 
// If compass heading is 0 the head of the copter is in north direction.
// A positive pitch angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative pitch).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
 
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
 
// rotation transformation to compass heading to match any copter orientation
if (CompassHeading < 0) // no valid compass data
{ // disable GPS
GPS_Neutral();
}
else
{
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
}
// limit GPS controls
#define GPS_CTRL_LIMIT 35
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;
}
 
 
void GPS_Main(void)
{
static uint8_t GPS_Task = TSK_IDLE;
 
 
46,14 → 112,13
// poti2 enables the gps feature
if(Poti2 < 70) GPS_Task = TSK_IDLE;
else if (Poti2 < 160) GPS_Task = TSK_HOLD;
//else GPS_Task = TSK_HOME;
else GPS_Task = TSK_HOME; // Poti2 >= 160
 
 
switch(GPSInfo.status)
{
case INVALID: // invalid gps data
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary
break;
case PROCESSED: // if gps data are already processed
63,8 → 128,7
// and therefore disable GPS
else
{
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
GPSInfo.status = INVALID;
}
break;
80,8 → 144,7
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
// disable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
break; // eof TSK_IDLE
case TSK_HOLD:
// if sticks are centered and hold position is valid enable position hold control
93,46 → 156,7
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
 
//Calculate PD-components of the controller (negative sign for compensation)
P_North = -(GPS_P_Factor * GPSPosDev_North)/2048;
D_North = -(GPS_D_Factor * GPSInfo.velnorth)/512;
P_East = -(GPS_P_Factor * GPSPosDev_East)/2048;
D_East = -(GPS_D_Factor * GPSInfo.veleast)/512;
 
// PD-controller
PD_North = P_North + D_North;
PD_East = P_East + D_East;
 
// GPS to pitch and roll settings
 
//A positive pitch angle moves head downwards (flying forward).
//A positive roll angle tilts left side downwards (flying left).
 
// If compass heading is 0 the head of the copter is in north direction.
// A positive pitch angle will fly to north and a positive roll angle will fly to west.
// In case of a positive north deviation/velocity the
// copter should fly to south (negative pitch).
// In case of a positive east position deviation and a positive east velocity the
// copter should fly to west (positive roll).
 
// The influence of the GPS_Pitch and GPS_Roll variable is contrarily to the stick values
// in the fc.c. Therefore a positive north deviation/velocity should result in a positive
// GPS_Pitch and a positive east deviation/velocity should result in a negative GPS_Roll.
 
// rotation transformation to compass heading to match any copter orientation
if (CompassHeading < 0) // no valid compass data
{ // disable GPS
GPS_Task = TSK_IDLE;
GPS_Pitch = 0;
GPS_Roll = 0;
}
else
{
coscompass = (int32_t)c_cos_8192(CompassHeading);
sincompass = (int32_t)c_sin_8192(CompassHeading);
GPS_Roll = (int16_t)((coscompass * PD_East - sincompass * PD_North) / 8192);
GPS_Pitch = -1*(int16_t)((sincompass * PD_East + coscompass * PD_North) / 8192);
}
GPS_PDController();
}
else // MK controlled by user
{
141,29 → 165,40
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
// disable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
}
break; // eof TSK_HOLD
case TSK_HOME:
if(HomePosition.Status == VALID)
{
// update hold point to current gps position
HoldPosition.Northing = GPSInfo.utmnorth;
HoldPosition.Easting = GPSInfo.utmeast;
HoldPosition.Status = VALID;
 
// Calculate deviation from home position
GPSPosDev_North = GPSInfo.utmnorth - HomePosition.Northing;
GPSPosDev_East = GPSInfo.utmeast - HomePosition.Easting;
DebugOut.Analog[12] = GPSPosDev_North;
DebugOut.Analog[13] = GPSPosDev_East;
 
GPS_PDController();
}
else // bad home position
{
GPS_Neutral();
BeepTime = 50; // signal invalid home position
}
break; // eof TSK_HOME
default: // unhandled task
GPS_Task = TSK_IDLE;
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
break; // eof default
} // eof switch GPS_Task
 
// limit GPS controls
#define GPS_CTRL_LIMIT 35
if (GPS_Pitch > GPS_CTRL_LIMIT) GPS_Pitch = GPS_CTRL_LIMIT;
if (GPS_Pitch < -GPS_CTRL_LIMIT) GPS_Pitch = -GPS_CTRL_LIMIT;
if (GPS_Roll > GPS_CTRL_LIMIT) GPS_Roll = GPS_CTRL_LIMIT;
if (GPS_Roll < -GPS_CTRL_LIMIT) GPS_Roll = -GPS_CTRL_LIMIT;
} // eof 3D-FIX
 
else // no 3D-SATFIX
{ // disable gps control
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
if(GPS_Task != TSK_IDLE) BeepTime = 50;
}
 
/branches/V0.68d Code Redesign killagreg/fc.c
205,8 → 205,7
TurnOver180Pitch = (int32_t) ParamSet.AngleTurnOverPitch * 2500L;
TurnOver180Roll = (int32_t) ParamSet.AngleTurnOverRoll * 2500L;
ExternHightValue = 0;
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
}
 
/************************************************************************/
606,6 → 605,7
Reading_IntegralGyroRoll2 = IntegralRoll;
SumPitch = 0;
SumRoll = 0;
GPS_SetHomePosition();
}
}
else delay_startmotors = 0; // reset delay timer if sticks are not in this position
1056,8 → 1056,7
}
else
{
GPS_Pitch = 0;
GPS_Roll = 0;
GPS_Neutral();
}
 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
/branches/V0.68d Code Redesign killagreg/gps.h
10,6 → 10,8
extern uint8_t GPS_D_Factor;
 
extern void GPS_Main(void);
extern void GPS_SetHomePosition(void);
extern void GPS_Neutral(void);
 
#endif //_GPS_H
 
/branches/V0.68d Code Redesign killagreg/menu.c
204,8 → 204,8
LCD_printfxy(0,0,"Satfix: Unknown");
break;
}
LCD_printfxy(0,1,"Lon: %d.%d deg",GPSInfo.longitude/10000000L, (int16_t)(GPSInfo.longitude/100000L)%100);
LCD_printfxy(0,2,"Lat: %d.%d deg",GPSInfo.latitude/10000000L, GPSInfo.latitude%10000000L);
LCD_printfxy(0,1,"Lon: %d.%d deg",GPSInfo.longitude/10000000L, (GPSInfo.longitude%10000000L)/10000L);
LCD_printfxy(0,2,"Lat: %d.%d deg",GPSInfo.latitude/10000000L, (GPSInfo.latitude%10000000L)/10000L);
LCD_printfxy(0,3,"Alt: %d.%d m",GPSInfo.altitude/1000L,GPSInfo.altitude%1000L);
}
break;
/branches/V0.68d Code Redesign killagreg/mymath.c
77,3 → 77,23
else return (angle - 180); // ( (x < 0) && (y < 0)) 3rd quadrant
}
 
 
 
// integer square root
uint32_t c_sqrt(uint32_t number)
{
uint32_t s1, s2;
uint8_t iter = 0;
// initialization of iteration
s2 = number;
do // iterative formula to solve x^2 - n = 0
{
s1 = s2;
s2 = number / s1;
s2 += s1;
s2 /= 2;
iter++;
//if(iter > 40) break;
}while( ( (s1-s2) > 1) && (iter < 40));
return s2;
}
/branches/V0.68d Code Redesign killagreg/mymath.h
6,5 → 6,6
extern int16_t c_sin_8192(int16_t angle);
extern int16_t c_cos_8192(int16_t angle);
extern int16_t c_atan2(int16_t y, int16_t x);
extern uint32_t c_sqrt(uint32_t number);
 
#endif // _MYMATH_H