| 4,7 → 4,13 |
|---|
| #include "ubx.h" |
| #include "mymath.h" |
| #include "timer0.h" |
| #include "uart.h" |
| |
| #define TSK_IDLE 0 |
| #define TSK_HOLD 1 |
| #define TSK_HOME 2 |
| |
| |
| int16_t GPS_Pitch = 0; |
| int16_t GPS_Roll = 0; |
| |
| 20,10 → 26,11 |
|---|
| } GPS_Pos_t; |
| |
| // GPS coordinates for hold position |
| GPS_Pos_t GPSHoldPoint = {0,0, INVALID}; |
| GPS_Pos_t HoldPosition = {0,0, INVALID}; |
| // GPS coordinates for home position |
| GPS_Pos_t HomePosition = {0,0, INVALID}; |
| |
| |
| |
| // --------------------------------------------------------------------------------- |
| |
| |
| 33,16 → 40,21 |
|---|
| 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; |
| static uint8_t GPS_Task = TSK_IDLE; |
| |
| |
| // poti2 enables the gps feature |
| if((Poti2 > 70) && (!EmergencyLanding)) // run GPS function only if Poti 2 is larger than 70 (switch on) |
| { |
| if(Poti2 < 70) GPS_Task = TSK_IDLE; |
| else if (Poti2 < 160) GPS_Task = TSK_HOLD; |
| //else GPS_Task = TSK_HOME; |
| |
| |
| switch(GPSInfo.status) |
| { |
| case INVALID: // invalid gps data |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| BeepTime = 50; |
| if(GPS_Task != TSK_IDLE) BeepTime = 50; // beep if signal is neccesary |
| break; |
| case PROCESSED: // if gps data are already processed |
| // downcount timeout |
| 60,12 → 72,26 |
|---|
| // if the gps data quality is sufficient |
| 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; |
| // disable gps control |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| break; // eof TSK_IDLE |
| case TSK_HOLD: |
| // if sticks are centered and hold position is valid enable position hold control |
| if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (GPSHoldPoint.Status == VALID)) |
| if ((MaxStickPitch < 20) && (MaxStickRoll < 20) && (HoldPosition.Status == VALID)) |
| { |
| // Calculate deviation from hold position |
| GPSPosDev_North = GPSInfo.utmnorth - GPSHoldPoint.Northing; |
| GPSPosDev_East = GPSInfo.utmeast - GPSHoldPoint.Easting; |
| GPSPosDev_North = GPSInfo.utmnorth - HoldPosition.Northing; |
| GPSPosDev_East = GPSInfo.utmeast - HoldPosition.Easting; |
| 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; |
| 94,41 → 120,58 |
|---|
| // 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); |
| |
| // 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; |
| } |
| } |
| else // MK controlled by user |
| { |
| // update hold point to current gps position |
| GPSHoldPoint.Northing = GPSInfo.utmnorth; |
| GPSHoldPoint.Easting = GPSInfo.utmeast; |
| GPSHoldPoint.Status = VALID; |
| HoldPosition.Northing = GPSInfo.utmnorth; |
| HoldPosition.Easting = GPSInfo.utmeast; |
| HoldPosition.Status = VALID; |
| // disable gps control |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| } |
| break; // eof TSK_HOLD |
| default: // unhandled task |
| GPS_Task = TSK_IDLE; |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| 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 |
| { // diable gps control |
| { // disable gps control |
| GPS_Pitch = 0; |
| GPS_Roll = 0; |
| BeepTime = 50; |
| if(GPS_Task != TSK_IDLE) BeepTime = 50; |
| } |
| |
| // set current data as processed to avoid further calculations on the same gps data |
| GPSInfo.status = PROCESSED; |
| break; |
| } // eof GPSInfo.status |
| DebugOut.Analog[14] = GPS_Pitch; |
| DebugOut.Analog[15] = GPS_Roll; |
| } |
| return; |
| } |
| |