Subversion Repositories FlightCtrl

#include <stdlib.h>

#include <stdlib.h>

#include "fc.h"

#include "fc.h"

#include "ubx.h"

#include "ubx.h"

#include "mymath.h"

#include "mymath.h"

#include "timer0.h"

#include "timer0.h"

#include "rc.h"

#include "rc.h"

#include "eeprom.h"

#include "eeprom.h"

typedef enum

typedef enum

        GPS_FLIGHT_MODE_FREE,

        GPS_FLIGHT_MODE_FREE,

        GPS_FLIGHT_MODE_AID,

        GPS_FLIGHT_MODE_AID,

        GPS_FLIGHT_MODE_HOME,

        GPS_FLIGHT_MODE_HOME,

} FlightMode_t;

} FlightMode_t;

#define GPS_STICK_LIMIT         200             // limit of gps stick control to avoid critical flight attitudes

#define GPS_STICK_LIMIT         500             // limit of gps stick control to avoid critical flight attitudes

#define GPS_P_LIMIT                     100

#define GPS_P_LIMIT                     250

// calculates the GPS control stick values from the deviation to target position

// 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

// if the pointer to the target positin is NULL or is the target position invalid

// then the P part of the controller is deactivated.

// then the P part of the controller is deactivated.

void GPS_PIDController(GPS_Pos_t *pTargetPos)

void GPS_PIDController(GPS_Pos_t *pTargetPos)

{

{

        int32_t coscompass, sincompass;

        int32_t coscompass, sincompass;

        int32_t GPSPosDev_North, GPSPosDev_East; // Position deviation in cm

        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 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;

        int32_t PID_North = 0, PID_East = 0;

        static int32_t cos_target_latitude = 1;

        static int32_t cos_target_latitude = 1;

                        // reset error integral

                        // reset error integral

                        GPSPosDevIntegral_North = 0;

                        GPSPosDevIntegral_North = 0;

                        GPSPosDevIntegral_East = 0;

                        GPSPosDevIntegral_East = 0;

                }

                }

                //Calculate PID-components of the controller (negative sign for compensation)

                //Calculate PID-components of the controller

                P_North = -((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512;

                P_North = ((int32_t)FCParam.NaviGpsP * GPSPosDev_North)/512;

                // I-Part

                // I-Part

                I_North = -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048;

                I_North = ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_North)/2048;

                I_East =  -((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048;

                I_East =  ((int32_t)FCParam.NaviGpsI * GPSPosDevIntegral_East)/2048;

                        if( GPSPosDevIntegral_East > GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = GPS_POSDEV_INTEGRAL_LIMIT;

                        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;

                        else if (GPSPosDevIntegral_East < -GPS_POSDEV_INTEGRAL_LIMIT) GPSPosDevIntegral_East = -GPS_POSDEV_INTEGRAL_LIMIT;

                }

                }

                D_East =  -((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128;

                D_East =  ((int32_t)FCParam.NaviGpsD * GPSInfo.veleast) / 128;

                // combine PI- and D-Part

                // combine PI- and D-Part

                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive

                // in the fc.c. Therefore a positive north deviation/velocity should result in a positive

                // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.

                // GPS_Nick and a positive east deviation/velocity should result in a negative GPS_Roll.

                coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);

                coscompass = (int32_t)c_cos_8192(YawGyroHeading / YAW_GYRO_DEG_FACTOR);

                PID_Nick =   -1*((sincompass * PID_East + coscompass * PID_North) / 8192);

                // limit resulting GPS control vector

                // limit resulting GPS control vector

void GPS_Main(void)

void GPS_Main(void)

        static uint8_t GPS_P_Delay = 0;

        static uint8_t GPS_P_Delay = 0;

        uint16_t beep_rythm = 0;

        static uint16_t beep_rythm = 0;

        GPS_UpdateParameter();

        GPS_UpdateParameter();

        // store home position if start of flight flag is set

        // store home position if start of flight flag is set

        if(MKFlags & MKFLAG_CALIBRATE)

        if(MKFlags & MKFLAG_CALIBRATE)

        {

        {

                GPS_SetCurrPosition(&HomePosition);

                if(GPS_SetCurrPosition(&HomePosition)) BeepTime = 700;