Subversion Repositories NaviCtrl

Rev

Rev 131 | Rev 146 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 131 Rev 133
Line 132... Line 132...
132 
 
 132 
 
133 
//-------------------------------------------------------------
 133 
//-------------------------------------------------------------
134 
// Update GPSParamter
 134 
// Update GPSParamter
135 
void GPS_UpdateParameter(void)
 135 
void GPS_UpdateParameter(void)
- 
 
 136 
{
- 
 
 137 
        #define SWITCH_DELAY 500
136 
{
 138 
        static u32 SwitchDelay = 0;
- 
 
 139 
        static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF;
137 
        static GPS_FlightMode_t FlightMode_Old = GPS_FLIGHT_MODE_UNDEF;
 140 
 
138 
        // in case of bad receiving conditions
 141 
        // in case of bad receiving conditions
139 
        if(FC.RC_Quality < 100)
 142 
        if(FC.RC_Quality < 100)
140 
        {       // set fixed parameter
 143 
        {       // set fixed parameter
141 
                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
 144 
                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
Line 163... Line 166...
163 
                if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
 166 
                if(StopNavigation) GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
164 
                else
 167 
                else
165 
                {
 168 
                {
166 
                        if(Parameter.NaviGpsModeControl <  50)
 169 
                        if(Parameter.NaviGpsModeControl <  50)
167 
                        {
 170 
                        {
- 
 
 171 
                                if(FlightMode_Old == GPS_FLIGHT_MODE_FREE) SetDelay(SWITCH_DELAY);     
- 
 
 172 
                                if(CheckDelay(SwitchDelay))
- 
 
 173 
                                {      
168 
                                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
 174 
                                        GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_FREE;
169 
                                NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
 175 
                                        NCFlags &= ~(NC_FLAG_PH | NC_FLAG_CH);
170 
                                NCFlags |= NC_FLAG_FREE;
 176 
                                        NCFlags |= NC_FLAG_FREE;
- 
 
 177 
                                }
171 
                        }
 178 
                        }
172 
                        else if(Parameter.NaviGpsModeControl < 180)
 179 
                        else if(Parameter.NaviGpsModeControl < 180)
173 
                        {
 180 
                        {
- 
 
 181 
                                if(FlightMode_Old == GPS_FLIGHT_MODE_AID) SetDelay(SWITCH_DELAY);
- 
 
 182 
                                if(CheckDelay(SwitchDelay))
- 
 
 183 
                                {
174 
                                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
 184 
                                        GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_AID;
175 
                                NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH);
 185 
                                        NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_CH);
176 
                                NCFlags |= NC_FLAG_PH;
 186 
                                        NCFlags |= NC_FLAG_PH;
- 
 
 187 
                                }
177 
                        }
 188 
                        }
178 
                        else
 189 
                        else
179 
                        {
 190 
                        {
- 
 
 191 
                                if(FlightMode_Old == GPS_FLIGHT_MODE_WAYPOINT) SetDelay(SWITCH_DELAY);
- 
 
 192 
                                if(CheckDelay(SwitchDelay))
- 
 
 193 
                                {
180 
                                GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
 194 
                                        GPS_Parameter.FlightMode = GPS_FLIGHT_MODE_WAYPOINT;
181 
                                NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
 195 
                                        NCFlags &= ~(NC_FLAG_FREE | NC_FLAG_PH);
182 
                                NCFlags |= NC_FLAG_CH;
 196 
                                        NCFlags |= NC_FLAG_CH;
- 
 
 197 
                                }
183 
                        }
 198 
                        }
184 
                }
 199 
                }
185 
                GPS_Parameter.Gain      = (float)Parameter.NaviGpsGain;
 200 
                GPS_Parameter.Gain      = (float)Parameter.NaviGpsGain;
186 
                GPS_Parameter.P         = (float)Parameter.NaviGpsP;
 201 
                GPS_Parameter.P         = (float)Parameter.NaviGpsP;
187 
                GPS_Parameter.I         = (float)Parameter.NaviGpsI;
 202 
                GPS_Parameter.I         = (float)Parameter.NaviGpsI;
Line 200... Line 215...
200 
                GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
 215 
                GPS_Parameter.OperatingRadius = (s32)Parameter.NaviOperatingRadius * 100; // conversion of m to cm
201 
        }
 216 
        }
202 
        // FlightMode changed?
 217 
        // FlightMode changed?
203 
        if(GPS_Parameter.FlightMode != FlightMode_Old)
 218 
        if(GPS_Parameter.FlightMode != FlightMode_Old)
204 
        {
 219 
        {
205 
                BeepTime = 100; // beep to indicate that mode has been switched
 220 
                BeepTime = 200; // beep to indicate that mode has been switched
206 
                NCFlags &= ~NC_FLAG_TARGET_REACHED;
 221 
                NCFlags &= ~NC_FLAG_TARGET_REACHED;
207 
                // if the mode has changed to free
 222 
                // if the mode has changed to free
208 
                if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free
 223 
                if(GPS_Parameter.FlightMode == GPS_FLIGHT_MODE_FREE) WPList_Clear(); // clear WPList if mode has changed to Free
209 
        }
 224 
        }
210 
        FlightMode_Old = GPS_Parameter.FlightMode;
 225 
        FlightMode_Old = GPS_Parameter.FlightMode;
Line 487... Line 502...
487 
                                                        GPS_pTargetPosition = NULL;
 502 
                                                        GPS_pTargetPosition = NULL;
488 
                                                        GPS_TargetRadius = 0;
 503 
                                                        GPS_TargetRadius = 0;
489 
                                                }
 504 
                                                }
490 
                                                else
 505 
                                                else
491 
                                                {
 506 
                                                {
- 
 
 507 
                                                        /*                                                     
492 
                                                        #define PH_MOVE_THRESHOLD 8
 508 
                                                        #define PH_MOVE_THRESHOLD 8
493 
                                                        if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150))
 509 
                                                        if( ((abs(FC.StickNick) > PH_MOVE_THRESHOLD) || (abs(FC.StickRoll) > PH_MOVE_THRESHOLD) ) && (FC.RC_Quality > 150))
494 
                                                        {   // indirect control by moving the hold position with rc sticks
 510 
                                                        {   // indirect control by moving the hold position with rc sticks
495 
                                                                // rc sticks have a range of +/-127 counts
 511 
                                                                // rc sticks have a range of +/-127 counts
496 
                                                                // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm
 512 
                                                                // GPS Coordinates are in 1-E7 deg, i.e. 1 counts is 1E-7°/360°*40E+8cm = 1.11 cm
497 
                                                                // normal manual activtion threshold is 8 counts that results in a change of the hold position
 513 
                                                                // normal manual activtion threshold is 8 counts that results in a change of the hold position
498 
                                                                // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s
 514 
                                                                // of min 8*1.11cm * 5Hz = 8.88 = 44.4cm/s an max 127*1.11cm *5Hz = 705cm/s
499 
                                                                GPS_HoldPosition.Latitude +=  (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll);
 515 
                                                                GPS_HoldPosition.Latitude +=  (s32)(cos_h*FC.StickNick + sin_h*FC.StickRoll);
500 
                                                                GPS_HoldPosition.Longitude +=  (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) );
 516 
                                                                GPS_HoldPosition.Longitude +=  (s32)((sin_h*FC.StickNick - cos_h*FC.StickRoll) / cos(RadiansFromGPS(GPS_HoldPosition.Latitude)) );
501 
                                                        }
 517 
                                                        }*/
- 
 
 518 
                                                        // set target position
502 
                                                        GPS_pTargetPosition = &GPS_HoldPosition;
 519 
                                                        GPS_pTargetPosition = &GPS_HoldPosition;
503 
                                                        GPS_TargetRadius = 100; // 1 meter
 520 
                                                        GPS_TargetRadius = 100; // 1 meter
504 
                                                }
 521 
                                                }
505 
                                                break;
 522 
                                                break;