#include <inttypes.h>
#include <stdlib.h>
#include "fc.h"
#include "ubx.h"
#include "mymath.h"
#include "timer0.h"
#include "uart.h"
#include "rc.h"
#include "eeprom.h"
#define TSK_IDLE 0
#define TSK_HOLD 1
#define TSK_HOME 2
#define GPS_STICK_SENSE 15 // must be at least in a range where 90% of the trimming does not switch of the GPS function
#define GPS_STICK_LIMIT 45 // limit of gps stick control to avoid critical flight attitudes
#define GPS_POSDEV_INTEGRAL_LIMIT 32000 // limit for the position error integral
#define GPS_P_LIMIT 25
uint8_t GPS_P_Factor
= 0, GPS_I_Factor
= 0, GPS_D_Factor
= 0;
typedef struct
{
int32_t Longitude
;
int32_t Latitude
;
int32_t Altitude
;
uint8_t Status
;
} GPS_Pos_t
;
// GPS coordinates for hold position
GPS_Pos_t HoldPosition
= {0,0,0,INVALID
};
// GPS coordinates for home position
GPS_Pos_t HomePosition
= {0,0,0,INVALID
};
// ---------------------------------------------------------------------------------
// checks pitch and roll sticks for manual control
uint8_t IsManualControlled
(void)
{
if ( (abs(PPM_in
[ParamSet.
ChannelAssignment[CH_PITCH
]]) < GPS_STICK_SENSE
) && (abs(PPM_in
[ParamSet.
ChannelAssignment[CH_ROLL
]]) < GPS_STICK_SENSE
)) return 0;
else return 1;
}
// set home position to current positon
void GPS_SetHomePosition
(void)
{
if( ((GPSInfo.
status == VALID
) || (GPSInfo.
status == PROCESSED
)) && GPSInfo.
satfix == SATFIX_3D
)
{
HomePosition.
Longitude = GPSInfo.
longitude;
HomePosition.
Latitude = GPSInfo.
latitude;
HomePosition.
Altitude = GPSInfo.
altitude;
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.
Altitude = GPSInfo.
altitude;
HoldPosition.
Status = VALID
;
}
else
{
HoldPosition.
Status = INVALID
;
}
}
// clear home position
void GPS_ClearHomePosition
(void)
{
HomePosition.
Status = INVALID
;
}
// disable GPS control sticks
void GPS_Neutral
(void)
{
GPS_Pitch
= 0;
GPS_Roll
= 0;
}
// calculates the GPS control stick values from the deviation to target position
// if the pointer to the target positin is NULL or is the target position invalid
// then the P part of the controller is deactivated.
void GPS_PIDController
(GPS_Pos_t
*pTargetPos
)
{
int32_t temp
, temp1
, PID_Pitch
, PID_Roll
;
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, I_North
= 0, I_East
= 0;
int32_t PID_North
= 0, PID_East
= 0;
static int32_t cos_target_latitude
= 1;
static int32_t GPSPosDevIntegral_North
= 0, GPSPosDevIntegral_East
= 0;
static GPS_Pos_t
*pLastTargetPos
= 0;
// if GPS data and Compass are ok
if((GPSInfo.
status == VALID
) && (GPSInfo.
satfix == SATFIX_3D
) && (CompassHeading
>= 0) )
{
if(pTargetPos
!= NULL
) // if there is a target position
{
if(pTargetPos
->Status
!= INVALID
) // and the position data are valid
{
// if the target data are updated or the target pointer has changed
if ((pTargetPos
->Status
!= PROCESSED
) || (pTargetPos
!= pLastTargetPos
) )
{
// reset error integral
GPSPosDevIntegral_North
= 0;
GPSPosDevIntegral_East
= 0;
// recalculate latitude projection
cos_target_latitude
= (int32_t)c_cos_8192
((int16_t)(pTargetPos
->Latitude
/10000000L));
// remember last target pointer
pLastTargetPos
= pTargetPos
;
// mark data as processed
pTargetPos
->Status
= PROCESSED
;
}
// calculate position deviation from latitude and longitude differences
GPSPosDev_North
= (GPSInfo.
latitude - pTargetPos
->Latitude
); // to calculate real cm we would need *111/100 additionally
GPSPosDev_East
= (GPSInfo.
longitude - pTargetPos
->Longitude
); // to calculate real cm we would need *111/100 additionally
// calculate latitude projection
GPSPosDev_East
*= cos_target_latitude
;
GPSPosDev_East
/= 8192;
}
else // no valid target position available
{
// reset error
GPSPosDev_North
= 0;
GPSPosDev_East
= 0;
// reset error integral
GPSPosDevIntegral_North
= 0;
GPSPosDevIntegral_East
= 0;
}
}
else // no target position available
{
// reset error
GPSPosDev_North
= 0;
GPSPosDev_East
= 0;
// reset error integral
GPSPosDevIntegral_North
= 0;
GPSPosDevIntegral_East
= 0;
}
//Calculate PID-components of the controller (negative sign for compensation)
// P-Part
P_North
= -((int32_t)GPS_P_Factor
* GPSPosDev_North
)/2048;
P_East
= -((int32_t)GPS_P_Factor
* GPSPosDev_East
)/2048;
// I-Part
I_North
= -((int32_t)GPS_I_Factor
* GPSPosDevIntegral_North
)/8192;
I_East
= -((int32_t)GPS_I_Factor
* GPSPosDevIntegral_East
)/8192;
// combine P- & I-Part
PID_North
= P_North
+ I_North
;
PID_East
= P_East
+ I_East
;
//limit PI-Part to limit the max velocity
//temp1 = ((int32_t)GPS_D_Factor * MAX_VELOCITY)/512; // the PI-Part limit
temp
= (int32_t)c_sqrt
(PID_North
*PID_North
+ PID_East
*PID_East
); // the current PI-Part
if(temp
> GPS_P_LIMIT
) // P-Part limit is reached
{
// normalize P-part components to the P-Part limit
PID_North
= (PID_North
* GPS_P_LIMIT
)/temp
;
PID_East
= (PID_East
* GPS_P_LIMIT
)/temp
;
}
else // PI-Part under its limit
{
// update position error integrals
GPSPosDevIntegral_North
+= GPSPosDev_North
/16;
if( GPSPosDevIntegral_North
> GPS_POSDEV_INTEGRAL_LIMIT
) GPSPosDevIntegral_North
= GPS_POSDEV_INTEGRAL_LIMIT
;
else if (GPSPosDevIntegral_North
< -GPS_POSDEV_INTEGRAL_LIMIT
) GPSPosDevIntegral_North
= -GPS_POSDEV_INTEGRAL_LIMIT
;
GPSPosDevIntegral_East
+= GPSPosDev_East
/16;
if( GPSPosDevIntegral_East
> GPS_POSDEV_INTEGRAL_LIMIT
) GPSPosDevIntegral_East
= GPS_POSDEV_INTEGRAL_LIMIT
;
else if (GPSPosDevIntegral_East
< -GPS_POSDEV_INTEGRAL_LIMIT
) GPSPosDevIntegral_East
= -GPS_POSDEV_INTEGRAL_LIMIT
;
}
// D-Part
D_North
= -((int32_t)GPS_D_Factor
* GPSInfo.
velnorth)/512;
D_East
= -((int32_t)GPS_D_Factor
* GPSInfo.
veleast)/512;
// combine PI- and D-Part
PID_North
+= D_North
;
PID_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.
coscompass
= (int32_t)c_cos_8192
(CompassHeading
);
sincompass
= (int32_t)c_sin_8192
(CompassHeading
);
PID_Roll
= (coscompass
* PID_East
- sincompass
* PID_North
) / 8192;
PID_Pitch
= -1*((sincompass
* PID_East
+ coscompass
* PID_North
) / 8192);
// limit resulting GPS control vector
temp
= (int32_t)c_sqrt
(PID_Roll
*PID_Roll
+ PID_Pitch
*PID_Pitch
);
if (temp
> GPS_STICK_LIMIT
)
{
// normalize control vector components to the limit
PID_Roll
= (PID_Roll
* GPS_STICK_LIMIT
)/temp
;
PID_Pitch
= (PID_Pitch
* GPS_STICK_LIMIT
)/temp
;
}
GPS_Roll
= (int16_t)PID_Roll
;
GPS_Pitch
= (int16_t)PID_Pitch
;
}
else // invalid GPS data or bad compass reading
{
GPS_Neutral
(); // do nothing
// reset error integral
GPSPosDevIntegral_North
= 0;
GPSPosDevIntegral_East
= 0;
}
}
void GPS_Main
(uint8_t ctrl
)
{
static uint8_t GPS_Task
= TSK_IDLE
;
static uint8_t GPS_P_Delay
= 0;
int16_t satbeep
;
// ctrl enables the gps feature
if(ctrl
< 70) GPS_Task
= TSK_IDLE
;
else if (ctrl
< 160) GPS_Task
= TSK_HOLD
;
else GPS_Task
= TSK_HOME
; // ctrl >= 160
switch(GPSInfo.
status)
{
case INVALID
: // invalid gps data
GPS_Neutral
();
if(GPS_Task
!= TSK_IDLE
)
{
BeepTime
= 100; // beep if signal is neccesary
}
break;
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
// and therefore disable GPS
else
{
GPS_Neutral
();
GPSInfo.
status = INVALID
;
}
break;
case VALID
: // new valid data from gps device
// if the gps data quality is good
DebugOut.
Analog[29] = (int16_t)GPSInfo.
updatetime;
if (GPSInfo.
satfix == SATFIX_3D
)
{
switch(GPS_Task
) // check what's to do
{
case TSK_IDLE
:
// 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 != INVALID
)
{
if( IsManualControlled
() ) // MK controlled by user
{
// update hold point to current gps position
GPS_SetHoldPosition
();
// disable gps control
GPS_Neutral
();
GPS_P_Delay
= 0;
}
else // GPS control active
{
if(GPS_P_Delay
<7)
{ // delayed activation of P-Part for 8 cycles (8*0.25s = 2s)
GPS_P_Delay
++;
GPS_SetHoldPosition
(); // update hold point to current gps position
GPS_PIDController
(NULL
); // activates only the D-Part
}
else GPS_PIDController
(&HoldPosition
);// activates the P&D-Part
}
}
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 != INVALID
)
{
// update hold point to current gps position
// to avoid a flight back if home comming is deactivated
GPS_SetHoldPosition
();
if( IsManualControlled
() ) // MK controlled by user
{
GPS_Neutral
();
}
else // GPS control active
{
GPS_PIDController
(&HomePosition
);
}
}
else // bad home position
{
BeepTime
= 50; // signal invalid home position
// try to hold at least the position as a fallback option
if (HoldPosition.
Status != INVALID
)
{
if( IsManualControlled
() ) // MK controlled by user
{
GPS_Neutral
();
}
else // GPS control active
{
GPS_PIDController
(&HoldPosition
);
}
}
else
{ // try to catch a valid hold position
GPS_SetHoldPosition
();
GPS_Neutral
();
}
}
break; // eof TSK_HOME
default: // unhandled task
GPS_Neutral
();
break; // eof default
} // eof switch GPS_Task
} // eof 3D-FIX
else // no 3D-SATFIX
{ // disable gps control
GPS_Neutral
();
if(GPS_Task
!= TSK_IDLE
)
{
satbeep
= 1600 - (int16_t)GPSInfo.
satnum * 200; // is zero at 8 sats
if (satbeep
< 0) satbeep
= 0;
BeepTime
= 50 + (uint16_t)satbeep
; // max 1650 * 0.1 ms =
}
}
// set current data as processed to avoid further calculations on the same gps data
GPSInfo.
status = PROCESSED
;
break;
} // eof GPSInfo.status
}