Arduino Quadcopter

1#include "Arduino.h"
2#include "config.h"
3#include "def.h"
4#include   "types.h"
5#include "GPS.h"
6#include "Serial.h"
7#include "Sensors.h"
8#include   "MultiWii.h"
9#include "EEPROM.h"
10#include <math.h>
11
12#if GPS
13
14//Function   prototypes for other GPS functions
15//These perhaps could go to the gps.h file,   however these are local to the gps.cpp  
16static void GPS_bearing(int32_t* lat1,   int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* bearing);
17static void GPS_distance_cm(int32_t*   lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2,uint32_t* dist);
18static void   GPS_calc_velocity(void);
19static void GPS_calc_location_error( int32_t* target_lat,   int32_t* target_lng, int32_t* gps_lat, int32_t* gps_lng );
20static void GPS_calc_poshold(void);
21static   uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow);
22static void   GPS_calc_nav_rate(uint16_t max_speed);
23int32_t wrap_18000(int32_t ang);
24static   bool check_missed_wp(void);
25void GPS_calc_longitude_scaling(int32_t lat);
26static   void GPS_update_crosstrack(void);
27int32_t wrap_36000(int32_t ang);
28
29
30//   Leadig filter - TODO: rewrite to normal C instead of C++
31
32// Set up gps lag
33#if   defined(UBLOX) || defined (MTK_BINARY19)
34#define GPS_LAG 0.5f                          //UBLOX   GPS has a smaller lag than MTK and other
35#else
36#define GPS_LAG 1.0f                          //We   assumes that MTK GPS has a 1 sec lag
37#endif  
38
39static int32_t  GPS_coord_lead[2];               // Lead filtered gps coordinates
40
41class LeadFilter {
42public:
43     LeadFilter() :
44        _last_velocity(0) {
45    }
46
47    // setup min   and max radio values in CLI
48    int32_t         get_position(int32_t pos, int16_t   vel, float lag_in_seconds = 1.0);
49    void            clear() { _last_velocity   = 0; }
50
51private:
52    int16_t         _last_velocity;
53
54};
55
56int32_t   LeadFilter::get_position(int32_t pos, int16_t vel, float lag_in_seconds)
57{
58     int16_t accel_contribution = (vel - _last_velocity) * lag_in_seconds * lag_in_seconds;
59     int16_t vel_contribution = vel * lag_in_seconds;
60
61    // store velocity   for next iteration
62    _last_velocity = vel;
63
64    return pos + vel_contribution   + accel_contribution;
65}
66
67
68LeadFilter xLeadFilter;      // Long GPS lag   filter 
69LeadFilter yLeadFilter;      // Lat  GPS lag filter 
70
71typedef struct   PID_PARAM_ {
72  float kP;
73  float kI;
74  float kD;
75  float Imax;
76  }   PID_PARAM;
77  
78PID_PARAM posholdPID_PARAM;
79PID_PARAM poshold_ratePID_PARAM;
80PID_PARAM   navPID_PARAM;
81
82typedef struct PID_ {
83  float   integrator; // integrator   value
84  int32_t last_input; // last input for derivative
85  float   lastderivative;   // last derivative for low-pass filter
86  float   output;
87  float   derivative;
88}   PID;
89PID posholdPID[2];
90PID poshold_ratePID[2];
91PID navPID[2];
92
93int32_t   get_P(int32_t error, struct PID_PARAM_* pid) {
94  return (float)error * pid->kP;
95}
96
97int32_t   get_I(int32_t error, float* dt, struct PID_* pid, struct PID_PARAM_* pid_param)   {
98  pid->integrator += ((float)error * pid_param->kI) * *dt;
99  pid->integrator   = constrain(pid->integrator,-pid_param->Imax,pid_param->Imax);
100  return pid->integrator;
101}
102   
103int32_t get_D(int32_t input, float* dt, struct PID_* pid, struct PID_PARAM_*   pid_param) { // dt in milliseconds
104  pid->derivative = (input - pid->last_input)   / *dt;
105
106  /// Low pass filter cut frequency for derivative calculation.
107   float filter = 7.9577e-3; // Set to  "1 / ( 2 * PI * f_cut )";
108  // Examples   for _filter:
109  // f_cut = 10 Hz -> _filter = 15.9155e-3
110  // f_cut = 15 Hz   -> _filter = 10.6103e-3
111  // f_cut = 20 Hz -> _filter =  7.9577e-3
112  // f_cut   = 25 Hz -> _filter =  6.3662e-3
113  // f_cut = 30 Hz -> _filter =  5.3052e-3
114
115   // discrete low pass filter, cuts out the
116  // high frequency noise that can   drive the controller crazy
117  pid->derivative = pid->lastderivative + (*dt / (   filter + *dt)) * (pid->derivative - pid->lastderivative);
118  // update state
119   pid->last_input = input;
120  pid->lastderivative    = pid->derivative;
121  //   add in derivative component
122  return pid_param->kD * pid->derivative;
123}
124
125void   reset_PID(struct PID_* pid) {
126  pid->integrator = 0;
127  pid->last_input = 0;
128   pid->lastderivative = 0;
129}
130
131#define _X 1
132#define _Y 0
133
134#define   RADX100                    0.000174532925  
135
136uint8_t land_detect;                 //Detect   land (extern)
137static uint32_t land_settle_timer;
138uint8_t GPS_Frame;            //   a valid GPS_Frame was detected, and data is ready for nav computation
139
140static   float  dTnav;            // Delta Time in milliseconds for navigation computations,   updated with every good GPS read
141static int16_t actual_speed[2] = {0,0};
142static   float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we   go towards the poles
143
144// The difference between the desired rate of travel   and the actual rate of travel
145// updated after GPS read - 5-10hz
146static int16_t   rate_error[2];
147static int32_t error[2];
148
149static int32_t GPS_WP[2];   //Currently   used WP
150static int32_t GPS_FROM[2]; //the pervious waypoint for precise track   following
151int32_t target_bearing;     // This is the angle from the copter to   the "next_WP" location in degrees * 100
152static int32_t original_target_bearing;   // deg * 100, The original angle to the next_WP when the next_WP was set, Also   used to check when we pass a WP
153static int16_t crosstrack_error;     // The amount   of angle correction applied to target_bearing to bring the copter back on its optimum   path
154uint32_t wp_distance;                // distance between plane and next_WP   in cm
155static uint16_t waypoint_speed_gov;  // used for slow speed wind up when   start navigation;
156
157
158////////////////////////////////////////////////////////////////////////////////////
159//   moving average filter variables
160//
161
162#define GPS_FILTER_VECTOR_LENGTH 5
163
164static   uint8_t GPS_filter_index = 0;
165static int32_t GPS_filter[2][GPS_FILTER_VECTOR_LENGTH];
166static   int32_t GPS_filter_sum[2];
167static int32_t GPS_read[2];
168static int32_t GPS_filtered[2];
169static   int32_t GPS_degree[2];    //the lat lon degree without any decimals (lat/10 000   000)
170static uint16_t fraction3[2];
171
172static int16_t nav_takeoff_bearing;   // saves the bearing at takeof (1deg = 1) used to rotate to takeoff direction   when arrives at home
173
174
175
176//Main navigation processor and state engine
177//   TODO: add proceesing states to ease processing burden 
178uint8_t GPS_Compute(void)   {
179  unsigned char axis;
180  uint32_t dist;        //temp variable to store dist   to copter
181  int32_t  dir;         //temp variable to store dir to copter
182   static uint32_t nav_loopTimer;
183
184  //check that we have a valid frame, if   not then return immediatly
185  if (GPS_Frame == 0) return 0; else GPS_Frame = 0;
186
187   //check home position and set it if it was not set
188  if (f.GPS_FIX && GPS_numSat   >= 5) {
189    #if !defined(DONT_RESET_HOME_AT_ARM)
190       if (!f.ARMED) {f.GPS_FIX_HOME   = 0;}
191    #endif
192    if (!f.GPS_FIX_HOME && f.ARMED) {
193      GPS_reset_home_position();
194     }
195    //Apply moving average filter to GPS data
196    if (GPS_conf.filtering)   {
197      GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
198       for (axis = 0; axis< 2; axis++) {
199        GPS_read[axis] = GPS_coord[axis];   //latest unfiltered data is in GPS_latitude and GPS_longitude
200        GPS_degree[axis]   = GPS_read[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t
201
202         // How close we are to a degree line ? its the first three digits from the   fractions of degree
203        // later we use it to Check if we are close to a   degree line, if yes, disable averaging,
204        fraction3[axis] = (GPS_read[axis]-   GPS_degree[axis]*10000000) / 10000;
205
206        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
207         GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000);   
208        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
209        GPS_filtered[axis]   = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
210         if ( NAV_state == NAV_STATE_HOLD_INFINIT || NAV_state == NAV_STATE_HOLD_TIMED)   {      //we use gps averaging only in poshold mode...
211          if ( fraction3[axis]>1   && fraction3[axis]<999 ) GPS_coord[axis] = GPS_filtered[axis];
212        }
213       }
214    }
215
216    //dTnav calculation
217    //Time for calculating x,y   speed and navigation pids
218    dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
219     nav_loopTimer = millis();
220
221    // prevent runup from bad GPS
222    dTnav   = min(dTnav, 1.0);  
223
224    //calculate distance and bearings for gui and other   stuff continously - From home to copter
225    GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dir);
226     GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist);
227     GPS_distanceToHome = dist/100;
228    GPS_directionToHome = dir/100;
229
230     if (!f.GPS_FIX_HOME) {     //If we don't have home set, do not display anything
231       GPS_distanceToHome = 0;
232      GPS_directionToHome = 0;
233    }
234
235     //Check fence setting and execute RTH if neccessary
236    //TODO: autolanding
237     if ((GPS_conf.fence > 0) && (GPS_conf.fence < GPS_distanceToHome) && (f.GPS_mode   != GPS_MODE_RTH) ) {
238      init_RTH();
239    }
240
241    //calculate the current   velocity based on gps coordinates continously to get a valid speed at the moment   when we start navigating
242    GPS_calc_velocity();        
243
244    //Navigation   state engine
245    if (f.GPS_mode != GPS_MODE_NONE) {   //ok we are navigating   ###0002 
246      //do gps nav calculations here, these are common for nav and poshold   
247      GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_bearing);
248       if (GPS_conf.lead_filter) {
249        GPS_distance_cm(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);
250         GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
251       } else {
252        GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);
253         GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
254       }
255
256      // Adjust altitude 
257      // if we are holding position   and reached target altitude, then ignore altitude nav, and let the user trim alt
258       if ( !((NAV_state == NAV_STATE_HOLD_INFINIT) && (alt_change_flag == REACHED_ALT)))   {
259        if (!f.LAND_IN_PROGRESS) {
260          alt_to_hold = get_new_altitude();
261           AltHold = alt_to_hold;
262        }
263      }
264
265      int16_t speed   = 0;                   //Desired navigation speed
266
267      switch(NAV_state)                     //Navigation state machine
268        {
269        case NAV_STATE_NONE:                //Just for clarity, do nothing when nav_state is none
270          break;
271
272         case NAV_STATE_LAND_START:
273          GPS_calc_poshold();              //Land   in position hold
274          land_settle_timer = millis();
275          NAV_state   = NAV_STATE_LAND_SETTLE;
276          break;
277
278        case NAV_STATE_LAND_SETTLE:
279           GPS_calc_poshold();
280          if (millis()-land_settle_timer > 5000)
281             NAV_state = NAV_STATE_LAND_START_DESCENT;
282          break;
283
284         case NAV_STATE_LAND_START_DESCENT:
285          GPS_calc_poshold();                //Land   in position hold
286          f.THROTTLE_IGNORED = 1;            //Ignore Throtte   stick input
287          f.GPS_BARO_MODE    = 1;            //Take control of BARO   mode
288          land_detect = 0;                   //Reset land detector
289          f.LAND_COMPLETED   = 0;
290          f.LAND_IN_PROGRESS = 1;            // Flag land process
291          NAV_state   = NAV_STATE_LAND_IN_PROGRESS;
292          break;
293
294        case NAV_STATE_LAND_IN_PROGRESS:
295           GPS_calc_poshold();
296          check_land();  //Call land detector
297           if (f.LAND_COMPLETED) {
298            nav_timer_stop = millis() + 5000;
299             NAV_state = NAV_STATE_LANDED;
300          }
301          break;
302
303         case NAV_STATE_LANDED:
304          // Disarm if THROTTLE stick is at minimum   or 5sec past after land detected
305          if (rcData[THROTTLE]<MINCHECK || nav_timer_stop   <= millis()) { //Throttle at minimum or 5sec passed.
306            go_disarm();
307             f.OK_TO_ARM = 0;                //Prevent rearming
308            NAV_state   = NAV_STATE_NONE;     //Disable position holding.... prevent flippover
309            f.GPS_BARO_MODE   = 0;
310            f.LAND_COMPLETED = 0;
311            f.LAND_IN_PROGRESS = 0;
312             land_detect = 0;
313            f.THROTTLE_IGNORED = 0;
314            GPS_reset_nav();
315           }
316          break;
317
318        case NAV_STATE_HOLD_INFINIT:        //Constant   position hold, no timer. Only an rcOption change can exit from this
319          GPS_calc_poshold();
320           break;
321
322        case NAV_STATE_HOLD_TIMED:
323          if (nav_timer_stop   == 0) {                         //We are start a timed poshold
324            nav_timer_stop   = millis() + 1000*nav_hold_time;  //Set when we will continue
325          } else   if (nav_timer_stop <= millis()) {           //did we reach our time limit ?
326             if (mission_step.flag != MISSION_FLAG_END) {
327              NAV_state   = NAV_STATE_PROCESS_NEXT;            //if yes then process next mission step
328             }
329            NAV_error = NAV_ERROR_TIMEWAIT;
330          }
331           GPS_calc_poshold();                                //BTW hold position   till next command
332          break;
333
334        case NAV_STATE_RTH_START:
335           if ((alt_change_flag == REACHED_ALT) || (!GPS_conf.wait_for_rth_alt))   {             //Wait until we reach RTH altitude
336            GPS_set_next_wp(&GPS_home[LAT],&GPS_home[LON],   &GPS_coord[LAT], &GPS_coord[LON]); //If we reached then change mode and start RTH
337             NAV_state = NAV_STATE_RTH_ENROUTE;
338            NAV_error = NAV_ERROR_NONE;
339           } else {
340            GPS_calc_poshold();                                                               //hold   position till we reach RTH alt
341            NAV_error = NAV_ERROR_WAIT_FOR_RTH_ALT;
342           }
343          break;
344
345        case NAV_STATE_RTH_ENROUTE:                                                  //Doing   RTH navigation
346          speed = GPS_calc_desired_speed(GPS_conf.nav_speed_max,   GPS_conf.slow_nav); 
347          GPS_calc_nav_rate(speed);
348          GPS_adjust_heading();
349           if ((wp_distance <= GPS_conf.wp_radius) || check_missed_wp()) {            //if   yes switch to poshold mode
350            if (mission_step.parameter1 == 0) NAV_state   = NAV_STATE_HOLD_INFINIT;
351            else NAV_state = NAV_STATE_LAND_START;                                    // if parameter 1 in RTH step is non 0 then land   at home
352            if (GPS_conf.nav_rth_takeoff_heading) { magHold = nav_takeoff_bearing;   }
353          } 
354          break;
355
356        case NAV_STATE_WP_ENROUTE:
357           speed = GPS_calc_desired_speed(GPS_conf.nav_speed_max, GPS_conf.slow_nav);   
358          GPS_calc_nav_rate(speed);
359          GPS_adjust_heading();
360
361           if ((wp_distance <= GPS_conf.wp_radius) || check_missed_wp()) {               //This   decides what happen when we reached the WP coordinates 
362            if (mission_step.action   == MISSION_LAND) {                                  //Autoland
363              NAV_state   = NAV_STATE_LAND_START;                                         //Start landing
364               set_new_altitude(alt.EstAlt);                                             //Stop   any altitude changes
365            } else if (mission_step.flag == MISSION_FLAG_END)   {                         //If this was the last mission step (flag set by the mission   planner), then switch to poshold
366              NAV_state = NAV_STATE_HOLD_INFINIT;
367               NAV_error = NAV_ERROR_FINISH;
368            } else if (mission_step.action   == MISSION_HOLD_UNLIM) {                     //If mission_step was POSHOLD_UNLIM   and we reached the position then switch to poshold unlimited
369              NAV_state   = NAV_STATE_HOLD_INFINIT;
370              NAV_error = NAV_ERROR_FINISH;
371            }   else if (mission_step.action == MISSION_HOLD_TIME) {                      //If mission_step   was a timed poshold then initiate timed poshold
372              nav_hold_time =   mission_step.parameter1;
373              nav_timer_stop = 0;                                                       //This   indicates that we are starting a timed poshold
374              NAV_state = NAV_STATE_HOLD_TIMED;
375             } else {
376              NAV_state = NAV_STATE_PROCESS_NEXT;                                       //Otherwise   process next step
377            }
378          }
379          break;
380
381        case   NAV_STATE_DO_JUMP:
382          if (jump_times < 0) {                                  //Jump   unconditionally (supposed to be -1) -10 should not be here
383            next_step   = mission_step.parameter1;
384            NAV_state = NAV_STATE_PROCESS_NEXT;
385           }
386          if (jump_times == 0) {
387            jump_times = -10;                                     //reset jump counter
388            if (mission_step.flag   == MISSION_FLAG_END) {         //If this was the last mission step (flag set by   the mission planner), then switch to poshold
389              NAV_state = NAV_STATE_HOLD_INFINIT;
390               NAV_error = NAV_ERROR_FINISH;
391            } else
392              NAV_state   = NAV_STATE_PROCESS_NEXT;
393          }
394
395          if (jump_times > 0) {                                   //if zero not reached do a jump
396            next_step   = mission_step.parameter1;
397            NAV_state = NAV_STATE_PROCESS_NEXT;
398             jump_times--;
399          }
400          break;
401
402        case   NAV_STATE_PROCESS_NEXT:                             //Processing next mission step
403           NAV_error = NAV_ERROR_NONE;
404          if (!recallWP(next_step)) { 
405             abort_mission(NAV_ERROR_WP_CRC);
406          } else {
407            switch(mission_step.action)
408               {
409              //Waypoiny and hold commands all starts with an   enroute status it includes the LAND command too
410              case MISSION_WAYPOINT:
411               case MISSION_HOLD_TIME:
412              case MISSION_HOLD_UNLIM:
413               case MISSION_LAND:
414                set_new_altitude(mission_step.altitude);
415                 GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON],   &GPS_prev[LAT], &GPS_prev[LON]);
416                if ((wp_distance/100) >= GPS_conf.safe_wp_distance)   abort_mission(NAV_ERROR_TOOFAR);
417                else NAV_state = NAV_STATE_WP_ENROUTE;
418                 GPS_prev[LAT] = mission_step.pos[LAT];  //Save wp coordinates for   precise route calc
419                GPS_prev[LON] = mission_step.pos[LON];
420                 break;
421              case MISSION_RTH:
422                f.GPS_head_set   = 0;
423                if (GPS_conf.rth_altitude == 0 && mission_step.altitude   == 0) //if config and mission_step alt is zero 
424                  set_new_altitude(alt.EstAlt);      // RTH returns at the actual altitude 
425                else {
426                  uint32_t   rth_alt;
427                  if (mission_step.altitude == 0) rth_alt = GPS_conf.rth_altitude   * 100;   //altitude in mission step has priority
428                  else rth_alt   = mission_step.altitude;
429
430                  if (alt.EstAlt < rth_alt) set_new_altitude(rth_alt);                      //BUt only if we are below it.
431                  else set_new_altitude(alt.EstAlt);
432                 }
433                NAV_state = NAV_STATE_RTH_START;
434                break;
435               case MISSION_JUMP:
436                if (jump_times == -10) jump_times   = mission_step.parameter2;
437                if (mission_step.parameter1 > 0 &&   mission_step.parameter1 < mission_step.number)
438                  NAV_state =   NAV_STATE_DO_JUMP;
439                else //Error situation, invalid jump target
440                   abort_mission(NAV_ERROR_INVALID_JUMP);
441                break;
442               case MISSION_SET_HEADING:
443                GPS_poi[LAT] = 0; GPS_poi[LON]   = 0; // zeroing this out clears the possible pervious set_poi
444                if   (mission_step.parameter1 < 0) f.GPS_head_set = 0;
445                else {
446                   f.GPS_head_set = 1;
447                  GPS_directionToPoi =   mission_step.parameter1;
448                } 
449                break;
450              case   MISSION_SET_POI:
451                GPS_poi[LAT] = mission_step.pos[LAT];
452                GPS_poi[LON]   = mission_step.pos[LON];
453                f.GPS_head_set = 1;
454                break;
455               default:                                  //if we got an unknown action   code abort mission and hold position
456                abort_mission(NAV_ERROR_INVALID_DATA);
457                 break;
458              }
459            next_step++; //Prepare   for the next step
460          }
461          break;
462        } // switch end
463     } //end of gps calcs ###0002 
464  }
465  return 1;
466} // End of GPS_compute
467
468//   Abort current mission with the given error code (switch to poshold_infinit)
469void   abort_mission(unsigned char error_code) {
470  GPS_set_next_wp(&GPS_coord[LAT],   &GPS_coord[LON],&GPS_coord[LAT], &GPS_coord[LON]);
471  NAV_error = error_code;
472   NAV_state = NAV_STATE_HOLD_INFINIT;
473}
474
475//Adjusting heading according   to settings - MAG mode must be enabled
476void GPS_adjust_heading() {
477  //TODO:   Add slow windup for large heading change
478  //This controls the heading
479  if   (f.GPS_head_set) { // We have seen a SET_POI or a SET_HEADING command
480    if   (GPS_poi[LAT] == 0)
481      magHold = wrap_18000((GPS_directionToPoi*100))/100;
482     else {
483      GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&GPS_directionToPoi);
484       GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&wp_distance);
485       magHold = GPS_directionToPoi /100;
486    }
487  } else {                                //   heading controlled by the standard defines
488    if (GPS_conf.nav_controls_heading)   {
489      if (GPS_conf.nav_tail_first) {
490        magHold = wrap_18000(target_bearing-18000)/100;
491       } else {
492        magHold = wrap_18000(target_bearing)/100;
493      }
494     }
495  }
496}
497
498#define LAND_DETECT_THRESHOLD 40      //Counts of land   situation
499#define BAROPIDMIN           -180     //BaroPID reach this if we landed.....
500
501//Check   if we landed or not
502void check_land() {
503  // detect whether we have landed   by watching for low climb rate and throttle control
504  if ( (abs(alt.vario) <   20) && (BaroPID < BAROPIDMIN)) {
505    if (!f.LAND_COMPLETED) {
506      if( land_detect   < LAND_DETECT_THRESHOLD) {
507        land_detect++;
508      } else {
509        f.LAND_COMPLETED   = 1;
510        land_detect = 0;
511      }
512    }
513  } else {
514    // we've   detected movement up or down so reset land_detector
515    land_detect = 0;
516     if(f.LAND_COMPLETED) {
517      f.LAND_COMPLETED = 0;
518    }
519  }
520}
521
522int32_t   get_altitude_error() {
523  return alt_to_hold - alt.EstAlt;
524}
525
526void clear_new_altitude()   {
527  alt_change_flag = REACHED_ALT;
528}
529
530void force_new_altitude(int32_t   _new_alt) {
531  alt_to_hold     = _new_alt;
532  target_altitude = _new_alt;
533   alt_change_flag = REACHED_ALT;
534}
535
536void set_new_altitude(int32_t _new_alt)   {
537  //Limit maximum altitude command
538  if(_new_alt > GPS_conf.nav_max_altitude*100)   _new_alt = GPS_conf.nav_max_altitude * 100;
539  if(_new_alt == alt.EstAlt){
540     force_new_altitude(_new_alt);
541    return;
542  }
543  // We start at the   current location altitude and gradually change alt
544  alt_to_hold = alt.EstAlt;
545   // for calculating the delta time
546  alt_change_timer = millis();
547  // save   the target altitude
548  target_altitude = _new_alt;
549  // reset our altitude   integrator
550  alt_change = 0;
551  // save the original altitude
552  original_altitude   = alt.EstAlt;
553  // to decide if we have reached the target altitude
554  if(target_altitude   > original_altitude){
555    // we are below, going up
556    alt_change_flag =   ASCENDING;
557  } else if(target_altitude < original_altitude){
558    // we are   above, going down
559    alt_change_flag = DESCENDING;
560  } else {
561    // No   Change
562    alt_change_flag = REACHED_ALT;
563  }
564}
565
566int32_t get_new_altitude()   {
567  // returns a new altitude which feeded into the alt.hold controller
568  if(alt_change_flag   == ASCENDING) {
569    // we are below, going up
570    if(alt.EstAlt >=  target_altitude)   alt_change_flag = REACHED_ALT; 
571    // we shouldn't command past our target
572     if(alt_to_hold >=  target_altitude) return target_altitude; 
573  } else if   (alt_change_flag == DESCENDING) {
574    // we are above, going down
575    if(alt.EstAlt   <=  target_altitude) alt_change_flag = REACHED_ALT;
576    // we shouldn't command   past our target
577    if(alt_to_hold <=  target_altitude) return target_altitude;
578   }
579  // if we have reached our target altitude, return the target alt
580  if(alt_change_flag   == REACHED_ALT) return target_altitude;
581
582  int32_t diff  = abs(alt_to_hold   - target_altitude);
583  // scale is how we generate a desired rate from the elapsed   time
584  // a smaller scale means faster rates
585  int8_t _scale = 4;
586
587   if (alt_to_hold < target_altitude) {
588    // we are below the target alt
589     if(diff < 200) _scale = 4;
590    else _scale = 3;
591  } else {
592    // we   are above the target, going down
593    if(diff < 400) _scale = 5;  //Slow down   if only 4meters above
594    if(diff < 100) _scale = 6;  //Slow down further if   within 1meter
595  }
596
597  // we use the elapsed time as our altitude offset
598   // 1000 = 1 sec
599  // 1000 >> 4 = 64cm/s descent by default
600  int32_t change   = (millis() - alt_change_timer) >> _scale;
601
602  if(alt_change_flag == ASCENDING){
603     alt_change += change;
604  } else {
605     alt_change -= change;
606  }
607   // for generating delta time
608  alt_change_timer = millis();
609
610  return   original_altitude + alt_change;
611}
612
613////////////////////////////////////////////////////////////////////////////////////
614//PID   based GPS navigation functions
615//Author : EOSBandi
616//Based on code and ideas   from the Arducopter team: Jason Short,Randy Mackay, Pat Hickey, Jose Julio, Jani   Hirvinen
617//Andrew Tridgell, Justin Beech, Adam Rivera, Jean-Louis Naudin, Roberto   Navoni
618
619//original constraint does not work with variables
620int16_t constrain_int16(int16_t   amt, int16_t low, int16_t high) {
621  return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
622}
623////////////////////////////////////////////////////////////////////////////////////
624//   this is used to offset the shrinking longitude as we go towards the poles
625//   It's ok to calculate this once per waypoint setting, since it changes a little within   the reach of a multicopter
626//
627void GPS_calc_longitude_scaling(int32_t lat)   {
628  GPS_scaleLonDown = cos(lat * 1.0e-7f * 0.01745329251f);
629}
630
631////////////////////////////////////////////////////////////////////////////////////
632//   Sets the waypoint to navigate, reset neccessary variables and calculate initial   values
633//
634void GPS_set_next_wp(int32_t* lat_to, int32_t* lon_to, int32_t*   lat_from, int32_t* lon_from) {
635  GPS_WP[LAT] = *lat_to;
636  GPS_WP[LON] = *lon_to;
637
638   GPS_FROM[LAT] = *lat_from;
639  GPS_FROM[LON] = *lon_from;
640
641  GPS_calc_longitude_scaling(*lat_to);
642
643   GPS_bearing(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&target_bearing);
644   GPS_distance_cm(&GPS_FROM[LAT],&GPS_FROM[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance);
645   GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_FROM[LAT],&GPS_FROM[LON]);
646   waypoint_speed_gov = GPS_conf.nav_speed_min;
647  original_target_bearing = target_bearing;
648
649}
650
651////////////////////////////////////////////////////////////////////////////////////
652//   Check if we missed the destination somehow
653//
654static bool check_missed_wp(void)   {
655  int32_t temp;
656  temp = target_bearing - original_target_bearing;
657  temp   = wrap_18000(temp);
658  return (abs(temp) > 10000);   // we passed the waypoint   by 100 degrees
659}
660
661////////////////////////////////////////////////////////////////////////////////////
662//   Get distance between two points in cm
663// Get bearing from pos1 to pos2, returns   an 1deg = 100 precision
664
665void GPS_bearing(int32_t* lat1, int32_t* lon1, int32_t*   lat2, int32_t* lon2, int32_t* bearing) {
666  int32_t off_x = *lon2 - *lon1;
667   int32_t off_y = (*lat2 - *lat1) / GPS_scaleLonDown;
668
669  *bearing = 9000 +   atan2(-off_y, off_x) * 5729.57795f;      //Convert the output redians to 100xdeg
670   if (*bearing < 0) *bearing += 36000;
671}
672
673void GPS_distance_cm(int32_t*   lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2,uint32_t* dist) {
674  float dLat   = (float)(*lat2 - *lat1);                                    // difference of latitude   in 1/10 000 000 degrees
675  float dLon = (float)(*lon2 - *lon1) * GPS_scaleLonDown;   //x
676  *dist = sqrt(sq(dLat) + sq(dLon)) * 1.11318845f;
677}
678
679//*******************************************************************************************************
680//   calc_velocity_and_filtered_position - velocity in lon and lat directions calculated   from GPS position
681//       and accelerometer data
682// lon_speed expressed in   cm/s.  positive numbers mean moving east
683// lat_speed expressed in cm/s.  positive   numbers when moving north
684// Note: we use gps locations directly to calculate   velocity instead of asking gps for velocity because
685//       this is more accurate   below 1.5m/s
686// Note: even though the positions are projected using a lead filter,   the velocities are calculated
687//       from the unaltered gps locations.  We   do not want noise from our lead filter affecting velocity
688//*******************************************************************************************************
689static   void GPS_calc_velocity(void){
690  static int16_t speed_old[2] = {0,0};
691  static   int32_t last[2] = {0,0};
692  static uint8_t init = 0;
693
694  if (init) {
695     float tmp = 1.0/dTnav;
696    actual_speed[_X] = (float)(GPS_coord[LON] - last[LON])   *  GPS_scaleLonDown * tmp;
697    actual_speed[_Y] = (float)(GPS_coord[LAT]  - last[LAT])   * tmp;
698
699    //TODO: Check unrealistic speed changes and signal navigation   about posibble gps signal degradation
700    if (!GPS_conf.lead_filter) {
701      actual_speed[_X]   = (actual_speed[_X] + speed_old[_X]) / 2;
702      actual_speed[_Y] = (actual_speed[_Y]   + speed_old[_Y]) / 2;
703
704      speed_old[_X] = actual_speed[_X];
705      speed_old[_Y]   = actual_speed[_Y];
706    }
707  }
708  init=1;
709
710  last[LON] = GPS_coord[LON];
711   last[LAT] = GPS_coord[LAT];
712
713  if (GPS_conf.lead_filter) {
714    GPS_coord_lead[LON]   = xLeadFilter.get_position(GPS_coord[LON], actual_speed[_X], GPS_LAG);
715    GPS_coord_lead[LAT]   = yLeadFilter.get_position(GPS_coord[LAT], actual_speed[_Y], GPS_LAG);
716  }
717}
718
719////////////////////////////////////////////////////////////////////////////////////
720//   Calculate a location error between two gps coordinates
721// Because we are using   lat and lon to do our distance errors here's a quick chart:
722//   100  = 1m
723//   1000  = 11m    = 36 feet
724//  1800  = 19.80m = 60 feet
725//  3000  = 33m
726//   10000  = 111m
727//
728static void GPS_calc_location_error( int32_t* target_lat,   int32_t* target_lng, int32_t* gps_lat, int32_t* gps_lng ) {
729  error[LON] = (float)(*target_lng   - *gps_lng) * GPS_scaleLonDown;  // X Error
730  error[LAT] = *target_lat - *gps_lat;   // Y Error
731}
732
733////////////////////////////////////////////////////////////////////////////////////
734//   Calculate nav_lat and nav_lon from the x and y error and the speed
735//
736// TODO:   check that the poshold target speed constraint can be increased for snappier poshold   lock
737static void GPS_calc_poshold(void) {
738  int32_t d;
739  int32_t target_speed;
740   uint8_t axis;
741  
742  for (axis=0;axis<2;axis++) {
743    target_speed = get_P(error[axis],   &posholdPID_PARAM); // calculate desired speed from lat/lon error
744    target_speed   = constrain(target_speed,-100,100);      // Constrain the target speed in poshold   mode to 1m/s it helps avoid runaways..
745    rate_error[axis] = target_speed -   actual_speed[axis]; // calc the speed error
746
747    nav[axis]      =
748        get_P(rate_error[axis],                                                &poshold_ratePID_PARAM)
749       +get_I(rate_error[axis]   + error[axis], &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
750
751     d = get_D(error[axis],                    &dTnav, &poshold_ratePID[axis], &poshold_ratePID_PARAM);
752
753     d = constrain(d, -2000, 2000);
754
755    // get rid of noise
756    if(abs(actual_speed[axis])   < 50) d = 0;
757
758    nav[axis] +=d;
759    // nav[axis]  = constrain(nav[axis],   -NAV_BANK_MAX, NAV_BANK_MAX);
760    nav[axis]  = constrain_int16(nav[axis], -GPS_conf.nav_bank_max,   GPS_conf.nav_bank_max);
761    navPID[axis].integrator = poshold_ratePID[axis].integrator;
762   }
763}
764
765////////////////////////////////////////////////////////////////////////////////////
766//   Calculate the desired nav_lat and nav_lon for distance flying such as RTH and WP
767//
768static   void GPS_calc_nav_rate( uint16_t max_speed) {
769  float trig[2];
770  int32_t target_speed[2];
771   int32_t tilt;
772  uint8_t axis;
773
774  GPS_update_crosstrack();
775  int16_t   cross_speed = crosstrack_error * (GPS_conf.crosstrack_gain / 100.0);  //check is   it ok ?
776  cross_speed = constrain(cross_speed,-200,200);
777  cross_speed = -cross_speed;
778
779   float temp = (9000l - target_bearing) * RADX100;
780  trig[_X] = cos(temp);
781   trig[_Y] = sin(temp);
782
783  target_speed[_X] = max_speed * trig[_X] - cross_speed   * trig[_Y];
784  target_speed[_Y] = cross_speed * trig[_X] + max_speed * trig[_Y];
785
786   for (axis=0;axis<2;axis++) {
787    rate_error[axis] = target_speed[axis] - actual_speed[axis];
788     rate_error[axis] = constrain(rate_error[axis],-1000,1000);
789    nav[axis]       =
790        get_P(rate_error[axis],                        &navPID_PARAM)
791        +get_I(rate_error[axis], &dTnav, &navPID[axis], &navPID_PARAM)
792       +get_D(rate_error[axis],   &dTnav, &navPID[axis], &navPID_PARAM);
793
794    // nav[axis] = constrain(nav[axis],-NAV_BANK_MAX,NAV_BANK_MAX);
795     nav[axis]  = constrain_int16(nav[axis], -GPS_conf.nav_bank_max, GPS_conf.nav_bank_max);
796     poshold_ratePID[axis].integrator = navPID[axis].integrator;
797  }
798}
799
800static   void GPS_update_crosstrack(void) {
801  // Crosstrack Error
802  // ----------------
803   // If we are too far off or too close we don't do track following
804  float temp   = (target_bearing - original_target_bearing) * RADX100;
805  crosstrack_error =   sin(temp) * wp_distance; // Meters we are off track line
806}
807
808////////////////////////////////////////////////////////////////////////////////////
809//   Determine desired speed when navigating towards a waypoint, also implement slow   
810// speed rampup when starting a navigation
811//
812//      |< WP Radius
813//       0  1   2   3   4   5   6   7   8m
814//      ...|...|...|...|...|...|...|...|
815//                 100  |  200     300     400cm/s
816//                 |                                        +|+
817//                  |< we should slow to 1 m/s as we hit the target
818//
819static   uint16_t GPS_calc_desired_speed(uint16_t max_speed, bool _slow) {
820  if(_slow){
821     max_speed = min(max_speed, wp_distance / 2);
822  } else {
823    max_speed   = min(max_speed, wp_distance);
824    max_speed = max(max_speed, GPS_conf.nav_speed_min);   // go at least nav_speed_min
825  }
826  // limit the ramp up of the speed
827   // waypoint_speed_gov is reset to 0 at each new WP command
828  if(max_speed >   waypoint_speed_gov){
829    waypoint_speed_gov += (int)(100.0 * dTnav); // increase   at .5/ms
830    max_speed = waypoint_speed_gov;
831  }
832  return max_speed;
833}
834
835////////////////////////////////////////////////////////////////////////////////////
836//   Utilities
837//
838
839int32_t wrap_36000(int32_t ang) {
840  if (ang > 36000) ang   -= 36000;
841  if (ang < 0)     ang += 36000;
842  return ang;
843}
844
845
846/*
847   * EOS increased the precision here, even if we think that the gps is not precise   enough, with 10e5 precision it has 76cm resolution
848 * with 10e7 it's around 1   cm now. Increasing it further is irrelevant, since even 1cm resolution is unrealistic,   however increased 
849 * resolution also increased precision of nav calculations
850*/
851
852#define   DIGIT_TO_VAL(_x)        (_x - '0')
853uint32_t GPS_coord_to_degrees(char* s) {
854   char *p, *q;
855  uint8_t deg = 0, min = 0;
856  unsigned int frac_min = 0;
857   uint8_t i;
858
859  // scan for decimal point or end of field
860  for (p = s;   isdigit(*p); p++) ;
861  q = s;
862
863  // convert degrees
864  while ((p - q)   > 2) {
865    if (deg)
866      deg *= 10;
867    deg += DIGIT_TO_VAL(*q++);
868   }
869  // convert minutes
870  while (p > q) {
871    if (min)
872      min *=   10;
873    min += DIGIT_TO_VAL(*q++);
874  }
875  // convert fractional minutes
876   // expect up to four digits, result is in
877  // ten-thousandths of a minute
878   if (*p == '.') {
879    q = p + 1;
880    for (i = 0; i < 4; i++) {
881      frac_min   *= 10;
882      if (isdigit(*q))
883        frac_min += *q++ - '0';
884    }
885   }
886  return deg * 10000000UL + (min * 1000000UL + frac_min*100UL) / 6;
887}
888
889//   helper functions 
890uint16_t grab_fields(char* src, uint8_t mult) {  // convert   string to uint16
891  uint8_t i;
892  uint16_t tmp = 0;
893
894  for(i=0; src[i]!=0;   i++) {
895    if(src[i] == '.') {
896      i++;
897      if(mult==0)   break;
898       else  src[i+mult] = 0;
899    }
900    tmp *= 10;
901    if(src[i] >='0' &&   src[i] <='9') tmp += src[i]-'0';
902  }
903  return tmp;
904}
905
906uint8_t hex_c(uint8_t   n) {    // convert '0'..'9','A'..'F' to 0..15
907  n -= '0';
908  if(n>9)  n -=   7;
909  n &= 0x0F;
910  return n;
911} 
912
913//************************************************************************
914//   Common GPS functions 
915//
916void init_RTH() {
917  f.GPS_mode = GPS_MODE_RTH;            // Set GPS_mode to RTH
918  f.GPS_BARO_MODE = true;
919  GPS_hold[LAT]   = GPS_coord[LAT];      //All RTH starts with a poshold 
920  GPS_hold[LON] = GPS_coord[LON];       //This allows to raise to rth altitude
921  GPS_set_next_wp(&GPS_hold[LAT],&GPS_hold[LON],   &GPS_hold[LAT], &GPS_hold[LON]);
922  NAV_paused_at = 0;
923  if (GPS_conf.rth_altitude   == 0) set_new_altitude(alt.EstAlt);     //Return at actual altitude
924  else {                                                             // RTH altitude is defined,   but we use it only if we are below it
925    if (alt.EstAlt < GPS_conf.rth_altitude   * 100) 
926      set_new_altitude(GPS_conf.rth_altitude * 100);
927    else set_new_altitude(alt.EstAlt);
928   }
929  f.GPS_head_set = 0;                                               //Allow   the RTH ti handle heading
930  NAV_state = NAV_STATE_RTH_START;                                  //NAV   engine status is Starting RTH.
931}
932
933void GPS_reset_home_position(void) {
934   if (f.GPS_FIX && GPS_numSat >= 5) {
935    GPS_home[LAT] = GPS_coord[LAT];
936     GPS_home[LON] = GPS_coord[LON];
937    GPS_calc_longitude_scaling(GPS_coord[LAT]);     //need an initial value for distance and bearing calc
938    nav_takeoff_bearing   = att.heading;             //save takeoff heading
939    //TODO: Set ground altitude
940     f.GPS_FIX_HOME = 1;
941  }
942}
943
944//reset navigation (stop the navigation   processor, and clear nav)
945void GPS_reset_nav(void) {
946  uint8_t i;
947
948   for(i=0;i<2;i++) {
949    nav[i] = 0;
950    reset_PID(&posholdPID[i]);
951    reset_PID(&poshold_ratePID[i]);
952     reset_PID(&navPID[i]);
953    NAV_state = NAV_STATE_NONE;
954    //invalidate   JUMP counter
955    jump_times = -10;
956    //reset next step counter
957    next_step   = 1;
958    //Clear poi
959    GPS_poi[LAT] = 0; GPS_poi[LON] = 0;
960    f.GPS_head_set   = 0;
961  }
962}
963
964//Get the relevant P I D values and set the PID controllers   
965void GPS_set_pids(void) {
966  posholdPID_PARAM.kP   = (float)conf.pid[PIDPOS].P8/100.0;
967   posholdPID_PARAM.kI   = (float)conf.pid[PIDPOS].I8/100.0;
968  posholdPID_PARAM.Imax   = POSHOLD_RATE_IMAX * 100;
969
970  poshold_ratePID_PARAM.kP   = (float)conf.pid[PIDPOSR].P8/10.0;
971   poshold_ratePID_PARAM.kI   = (float)conf.pid[PIDPOSR].I8/100.0;
972  poshold_ratePID_PARAM.kD    = (float)conf.pid[PIDPOSR].D8/1000.0;
973  poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX   * 100;
974
975  navPID_PARAM.kP   = (float)conf.pid[PIDNAVR].P8/10.0;
976  navPID_PARAM.kI    = (float)conf.pid[PIDNAVR].I8/100.0;
977  navPID_PARAM.kD   = (float)conf.pid[PIDNAVR].D8/1000.0;
978   navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
979  }
980//It was moved here since   even i2cgps code needs it
981int32_t wrap_18000(int32_t ang) {
982  if (ang > 18000)   ang -= 36000;
983  if (ang < -18000) ang += 36000;
984  return ang;
985}
986
987
988
989
990
991
992
993
994
995
996
997
998
999/**************************************************************************************/
1000/**************************************************************************************/
1001/***********************        specific  GPS device section  **************************/
1002/**************************************************************************************/
1003/**************************************************************************************/
1004
1005#if   defined(GPS_SERIAL)
1006
1007/**************************************************************************************/
1008/***********************        NMEA                          **************************/
1009/**************************************************************************************/
1010#if   defined(NMEA)
1011/* This is a light implementation of a GPS frame decoding
1012   This   should work with most of modern GPS devices configured to output NMEA frames.
1013    It assumes there are some NMEA GGA frames to decode on the serial bus
1014   Here   we use only the following data :
1015     - latitude
1016     - longitude
1017     -   GPS fix is/is not ok
1018     - GPS num sat (4 is enough to be +/- reliable)
1019      - GPS altitude
1020     - GPS speed
1021*/
1022#define FRAME_GGA  1
1023#define   FRAME_RMC  2
1024
1025void GPS_SerialInit(void) {
1026  SerialOpen(GPS_SERIAL,GPS_BAUD);
1027   delay(1000);
1028}
1029
1030bool GPS_newFrame(uint8_t c) {
1031  uint8_t frameOK =   0;
1032  static uint8_t param = 0, offset = 0, parity = 0;
1033  static char string[15];
1034   static uint8_t checksum_param, frame = 0;
1035
1036  if (c == '$') {
1037    param   = 0; offset = 0; parity = 0;
1038  } else if (c == ',' || c == '*') {
1039    string[offset]   = 0;
1040    if (param == 0) { //frame identification
1041      frame = 0;
1042      if   (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' &&   string[4] == 'A') frame = FRAME_GGA;
1043      if (string[0] == 'G' && string[1]   == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') frame = FRAME_RMC;
1044     } else if (frame == FRAME_GGA) {
1045      if      (param == 2)                     {GPS_coord[LAT]   = GPS_coord_to_degrees(string);}
1046      else if (param == 3 && string[0] == 'S')   GPS_coord[LAT] = -GPS_coord[LAT];
1047      else if (param == 4)                     {GPS_coord[LON]   = GPS_coord_to_degrees(string);}
1048      else if (param == 5 && string[0] == 'W')   GPS_coord[LON] = -GPS_coord[LON];
1049      else if (param == 6)                     {f.GPS_FIX   = (string[0]  > '0');}
1050      else if (param == 7)                     {GPS_numSat   = grab_fields(string,0);}
1051      else if (param == 9)                     {GPS_altitude   = grab_fields(string,0);}  // altitude in meters added by Mis
1052    } else if (frame   == FRAME_RMC) {
1053      if      (param == 7)                     {GPS_speed = ((uint32_t)grab_fields(string,1)*5144L)/1000L;}   //gps speed in cm/s will be used for navigation
1054      else if (param == 8)                      {GPS_ground_course = grab_fields(string,1); }                 //ground   course deg*10 
1055    }
1056    param++; offset = 0;
1057    if (c == '*') checksum_param=1;
1058     else parity ^= c;
1059  } else if (c == '\
' || c == '\
1060') {
1061    if (checksum_param)   { //parity checksum
1062      uint8_t checksum = hex_c(string[0]);
1063      checksum   <<= 4;
1064      checksum += hex_c(string[1]);
1065      if (checksum == parity) frameOK   = 1;
1066    }
1067    checksum_param=0;
1068  } else {
1069     if (offset < 15) string[offset++]   = c;
1070     if (!checksum_param) parity ^= c;
1071  }
1072  return frameOK && (frame==FRAME_GGA);
1073}
1074#endif   //NMEA
1075
1076
1077
1078/**************************************************************************************/
1079/***********************        UBLOX                         **************************/
1080/**************************************************************************************/
1081#if   defined(UBLOX)
1082const char UBLOX_INIT[] PROGMEM = {                                                  //   PROGMEM array must be outside any function !!!
1083  0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x05,0x00,0xFF,0x19,                             //disable all default NMEA messages
1084  0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x03,0x00,0xFD,0x15,
1085   0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x01,0x00,0xFB,0x11,
1086  0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x00,0x00,0xFA,0x0F,
1087   0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x02,0x00,0xFC,0x13,
1088  0xB5,0x62,0x06,0x01,0x03,0x00,0xF0,0x04,0x00,0xFE,0x17,
1089   0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x02,0x01,0x0E,0x47,                            //set   POSLLH MSG rate
1090  0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x03,0x01,0x0F,0x49,                            //set   STATUS MSG rate
1091  0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x06,0x01,0x12,0x4F,                            //set   SOL MSG rate
1092  0xB5,0x62,0x06,0x01,0x03,0x00,0x01,0x12,0x01,0x1E,0x67,                            //set   VELNED MSG rate
1093  0xB5,0x62,0x06,0x16,0x08,0x00,0x03,0x07,0x03,0x00,0x51,0x08,0x00,0x00,0x8A,0x41,    //set WAAS to EGNOS
1094  0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01,   0x00, 0x01, 0x00, 0xDE, 0x6A //set rate to 5Hz
1095};
1096
1097struct ubx_header {
1098   uint8_t preamble1;
1099  uint8_t preamble2;
1100  uint8_t msg_class;
1101  uint8_t   msg_id;
1102  uint16_t length;
1103};
1104struct ubx_nav_posllh {
1105  uint32_t time;   // GPS msToW
1106  int32_t longitude;
1107  int32_t latitude;
1108  int32_t altitude_ellipsoid;
1109   int32_t altitude_msl;
1110  uint32_t horizontal_accuracy;
1111  uint32_t vertical_accuracy;
1112};
1113struct   ubx_nav_solution {
1114  uint32_t time;
1115  int32_t time_nsec;
1116  int16_t week;
1117   uint8_t fix_type;
1118  uint8_t fix_status;
1119  int32_t ecef_x;
1120  int32_t ecef_y;
1121   int32_t ecef_z;
1122  uint32_t position_accuracy_3d;
1123  int32_t ecef_x_velocity;
1124   int32_t ecef_y_velocity;
1125  int32_t ecef_z_velocity;
1126  uint32_t speed_accuracy;
1127   uint16_t position_DOP;
1128  uint8_t res;
1129  uint8_t satellites;
1130  uint32_t   res2;
1131};
1132struct ubx_nav_velned {
1133  uint32_t time;  // GPS msToW
1134  int32_t   ned_north;
1135  int32_t ned_east;
1136  int32_t ned_down;
1137  uint32_t speed_3d;
1138   uint32_t speed_2d;
1139  int32_t heading_2d;
1140  uint32_t speed_accuracy;
1141   uint32_t heading_accuracy;
1142};
1143
1144enum ubs_protocol_bytes {
1145  PREAMBLE1   = 0xb5,
1146  PREAMBLE2 = 0x62,
1147  CLASS_NAV = 0x01,
1148  CLASS_ACK = 0x05,
1149   CLASS_CFG = 0x06,
1150  MSG_ACK_NACK = 0x00,
1151  MSG_ACK_ACK = 0x01,
1152  MSG_POSLLH   = 0x2,
1153  MSG_STATUS = 0x3,
1154  MSG_SOL = 0x6,
1155  MSG_VELNED = 0x12,
1156  MSG_CFG_PRT   = 0x00,
1157  MSG_CFG_RATE = 0x08,
1158  MSG_CFG_SET_RATE = 0x01,
1159  MSG_CFG_NAV_SETTINGS   = 0x24
1160};
1161enum ubs_nav_fix_type {
1162  FIX_NONE = 0,
1163  FIX_DEAD_RECKONING   = 1,
1164  FIX_2D = 2,
1165  FIX_3D = 3,
1166  FIX_GPS_DEAD_RECKONING = 4,
1167  FIX_TIME   = 5
1168};
1169enum ubx_nav_status_bits {
1170  NAV_STATUS_FIX_VALID = 1
1171};
1172
1173//   Receive buffer
1174static union {
1175  ubx_nav_posllh posllh;
1176  ubx_nav_solution   solution;
1177  ubx_nav_velned velned;
1178  uint8_t bytes[];
1179 } _buffer;
1180
1181uint32_t   init_speed[5] = {9600,19200,38400,57600,115200};
1182
1183static void SerialGpsPrint(const   char PROGMEM * str) {
1184  char b;
1185  while(str && (b = pgm_read_byte(str++)))   {
1186    SerialWrite(GPS_SERIAL, b); 
1187    delay(5);
1188  }
1189}
1190
1191void GPS_SerialInit(void)   {
1192  SerialOpen(GPS_SERIAL,GPS_BAUD);
1193  delay(1000);
1194  for(uint8_t i=0;i<5;i++){
1195     SerialOpen(GPS_SERIAL,init_speed[i]);          // switch UART speed for sending   SET BAUDRATE command (NMEA mode)
1196    #if (GPS_BAUD==19200)
1197      SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,19200,0*23\
\
1198"));      // 19200 baud - minimal speed for 5Hz update rate
1199    #endif  
1200    #if   (GPS_BAUD==38400)
1201      SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,38400,0*26\
\
1202"));      // 38400 baud
1203    #endif  
1204    #if (GPS_BAUD==57600)
1205      SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,57600,0*2D\
\
1206"));      // 57600 baud
1207    #endif  
1208    #if (GPS_BAUD==115200)
1209      SerialGpsPrint(PSTR("$PUBX,41,1,0003,0001,115200,0*1E\
\
1210"));     // 115200 baud
1211    #endif  
1212    while(!SerialTXfree(GPS_SERIAL)) delay(10);
1213   }
1214  delay(200);
1215  SerialOpen(GPS_SERIAL,GPS_BAUD);  
1216  for(uint8_t i=0;   i<sizeof(UBLOX_INIT); i++) {                        // send configuration data in   UBX protocol
1217    SerialWrite(GPS_SERIAL, pgm_read_byte(UBLOX_INIT+i));
1218    delay(5);   //simulating a 38400baud pace (or less), otherwise commands are not accepted by   the device.
1219  }
1220}
1221
1222bool GPS_newFrame(uint8_t data){
1223  static uint8_t   _step = 0; // State machine state
1224  static uint8_t  _msg_id;
1225  static uint16_t   _payload_length;
1226  static uint16_t _payload_counter;
1227  static uint8_t  _ck_a;   // Packet checksum accumulators
1228  static uint8_t  _ck_b;
1229
1230  uint8_t st   = _step+1;
1231  bool    ret = false;
1232  
1233  if (st == 2)
1234    if (PREAMBLE2   != data) st--; // in case of faillure of the 2nd header byte, still test the first   byte
1235  if (st == 1) {
1236    if(PREAMBLE1 != data) st--;
1237  } else if (st ==   3) { // CLASS byte, not used, assume it is CLASS_NAV
1238    _ck_b = _ck_a = data;   // reset the checksum accumulators
1239  } else if (st > 3 && st < 8) {
1240    _ck_b   += (_ck_a += data);  // checksum byte
1241    if (st == 4) {
1242      _msg_id = data;
1243     } else if (st == 5) {
1244      _payload_length = data; // payload length low   byte
1245    } else if (st == 6) {
1246      _payload_length += (uint16_t)(data<<8);
1247       if (_payload_length > 512) st = 0;
1248      _payload_counter = 0;  // prepare   to receive payload
1249    } else {
1250      if (_payload_counter+1 < _payload_length)   st--; // stay in the same state while data inside the frame
1251      if (_payload_counter   < sizeof(_buffer)) _buffer.bytes[_payload_counter] = data;  
1252      _payload_counter++;
1253     }
1254  } else if (st == 8) {
1255    if (_ck_a != data) st = 0;  // bad checksum
1256   } else if (st == 9) {
1257    st = 0;
1258    if (_ck_b == data) { // good checksum
1259       if (_msg_id == MSG_POSLLH) {
1260        if(f.GPS_FIX) {
1261          GPS_coord[LON]   = _buffer.posllh.longitude;
1262          GPS_coord[LAT] = _buffer.posllh.latitude;
1263           GPS_altitude   = _buffer.posllh.altitude_msl / 1000; //alt in m
1264          //GPS_time        = _buffer.posllh.time; //not used for the moment
1265        }
1266        ret=   true;        // POSLLH message received, allow blink GUI icon and LED, frame available   for nav computation
1267      } else if (_msg_id ==  MSG_SOL) {
1268        f.GPS_FIX   = 0;
1269        if((_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type   == FIX_3D || _buffer.solution.fix_type == FIX_2D)) f.GPS_FIX = 1;
1270        GPS_numSat   = _buffer.solution.satellites;
1271      } else if (_msg_id ==  MSG_VELNED) {
1272         GPS_speed         = _buffer.velned.speed_2d;  // cm/s
1273        GPS_ground_course   = (uint16_t)(_buffer.velned.heading_2d / 10000);  // Heading 2D deg * 100000 rescaled   to deg * 10 //not used for the moment
1274      }
1275    }
1276  }
1277  _step = st;
1278   return ret;
1279}
1280#endif //UBLOX
1281
1282
1283
1284/**************************************************************************************/
1285/***********************        MTK                           **************************/
1286/**************************************************************************************/
1287#if   defined(MTK_BINARY16) || defined(MTK_BINARY19)
1288
1289#define MTK_SET_BINARY          PSTR("$PGCMD,16,0,0,0,0,0*6A\
\
1290")
1291#define   MTK_SET_NMEA            PSTR("$PGCMD,16,1,1,1,1,1*6B\
\
1292")
1293#define MTK_SET_NMEA_SENTENCES   PSTR("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\
\
1294")
1295#define MTK_OUTPUT_4HZ           PSTR("$PMTK220,250*29\
\
1296")
1297#define MTK_OUTPUT_5HZ          PSTR("$PMTK220,200*2C\
\
1298")
1299#define   MTK_OUTPUT_10HZ         PSTR("$PMTK220,100*2F\
\
1300")
1301#define MTK_NAVTHRES_OFF         PSTR("$PMTK397,0*23\
\
1302") // Set Nav Threshold (the minimum speed the   GPS must be moving to update the position) to 0 m/s  
1303#define SBAS_ON                 PSTR("$PMTK313,1*2E\
\
1304")
1305#define   WAAS_ON                 PSTR("$PMTK301,2*2E\
\
1306")
1307#define SBAS_TEST_MODE           PSTR("$PMTK319,0*25\
\
1308")  //Enable test use of sbas satelite in test   mode (usually PRN124 is in test mode)
1309
1310struct diyd_mtk_msg {
1311  int32_t   latitude;
1312  int32_t  longitude;
1313  int32_t  altitude;
1314  int32_t  ground_speed;
1315   int32_t  ground_course;
1316  uint8_t  satellites;
1317  uint8_t  fix_type;
1318   uint32_t utc_date;
1319  uint32_t utc_time;
1320  uint16_t hdop;
1321};
1322
1323//   #pragma pack(pop)
1324enum diyd_mtk_fix_type {
1325  FIX_NONE = 1,
1326  FIX_2D = 2,
1327   FIX_3D = 3,
1328  FIX_2D_SBAS = 6,
1329  FIX_3D_SBAS = 7 
1330};
1331
1332#if defined(MTK_BINARY16)
1333enum   diyd_mtk_protocol_bytes {
1334  PREAMBLE1 = 0xd0,
1335  PREAMBLE2 = 0xdd,
1336};
1337#endif
1338
1339#if   defined(MTK_BINARY19)
1340enum diyd_mtk_protocol_bytes {
1341  PREAMBLE1 = 0xd1,
1342   PREAMBLE2 = 0xdd,
1343};
1344#endif
1345
1346// Packet checksum accumulators
1347uint8_t   _ck_a;
1348uint8_t  _ck_b;
1349
1350// State machine state
1351uint8_t  _step;
1352uint8_t   _payload_counter;
1353
1354// Time from UNIX Epoch offset
1355long  _time_offset;
1356bool   _offset_calculated;
1357
1358// Receive buffer
1359union {
1360  diyd_mtk_msg  msg;
1361   uint8_t       bytes[];
1362} _buffer;
1363
1364inline long _swapl(const void *bytes)   {
1365  const uint8_t *b = (const uint8_t *)bytes;
1366  union {
1367    long    v;
1368     uint8_t b[4];
1369  } u;
1370
1371  u.b[0] = b[3];
1372  u.b[1] = b[2];
1373  u.b[2]   = b[1];
1374  u.b[3] = b[0];
1375
1376  return(u.v);
1377}
1378
1379uint32_t init_speed[5]   = {9600,19200,38400,57600,115200};
1380
1381void SerialGpsPrint(const char PROGMEM   * str) {
1382  char b;
1383  while(str && (b = pgm_read_byte(str++))) {
1384    SerialWrite(GPS_SERIAL,   b);     
1385  }
1386}
1387
1388void GPS_SerialInit(void) {
1389  SerialOpen(GPS_SERIAL,GPS_BAUD);
1390   delay(1000);
1391  #if defined(INIT_MTK_GPS)                              // MTK   GPS setup
1392    for(uint8_t i=0;i<5;i++){
1393      SerialOpen(GPS_SERIAL,init_speed[i]);                 // switch UART speed for sending SET BAUDRATE command
1394      #if   (GPS_BAUD==19200)
1395        SerialGpsPrint(PSTR("$PMTK251,19200*22\
\
1396"));      // 19200 baud - minimal speed for 5Hz update rate
1397      #endif  
1398      #if   (GPS_BAUD==38400)
1399        SerialGpsPrint(PSTR("$PMTK251,38400*27\
\
1400"));      // 38400 baud
1401      #endif  
1402      #if (GPS_BAUD==57600)
1403        SerialGpsPrint(PSTR("$PMTK251,57600*2C\
\
1404"));      // 57600 baud
1405      #endif  
1406      #if (GPS_BAUD==115200)
1407        SerialGpsPrint(PSTR("$PMTK251,115200*1F\
\
1408"));     // 115200 baud
1409      #endif  
1410      while(!SerialTXfree(GPS_SERIAL)) delay(80);
1411     }
1412    // at this point we have GPS working at selected (via #define GPS_BAUD)   baudrate
1413    // So now we have to set the desired mode and update rate (which   depends on the NMEA or MTK_BINARYxx settings)
1414    SerialOpen(GPS_SERIAL,GPS_BAUD);
1415
1416     SerialGpsPrint(MTK_NAVTHRES_OFF);
1417      while(!SerialTXfree(GPS_SERIAL))   delay(80);
1418    SerialGpsPrint(SBAS_ON);
1419      while(!SerialTXfree(GPS_SERIAL))   delay(80);
1420    SerialGpsPrint(WAAS_ON);
1421      while(!SerialTXfree(GPS_SERIAL))   delay(80);
1422    SerialGpsPrint(SBAS_TEST_MODE);
1423      while(!SerialTXfree(GPS_SERIAL))   delay(80);
1424    SerialGpsPrint(MTK_OUTPUT_5HZ);           // 5 Hz update rate
1425
1426     #if defined(NMEA)
1427      SerialGpsPrint(MTK_SET_NMEA_SENTENCES); // only GGA   and RMC sentence
1428    #endif     
1429    #if defined(MTK_BINARY19) || defined(MTK_BINARY16)
1430       SerialGpsPrint(MTK_SET_BINARY);
1431    #endif
1432  #endif  // init_mtk_gps
1433}
1434
1435bool   GPS_newFrame(uint8_t data) {
1436  bool parsed = false;
1437
1438  restart:
1439  switch(_step)   {
1440    // Message preamble, class, ID detection
1441    //
1442    // If we fail   to match any of the expected bytes, we
1443    // reset the state machine and re-consider   the failed
1444    // byte as the first byte of the preamble.  This
1445    // improves   our chances of recovering from a mismatch
1446    // and makes it less likely that   we will be fooled by
1447    // the preamble appearing as data in some other message.
1448     //
1449    case 0:
1450      if(PREAMBLE1 == data)
1451        _step++;
1452      break;
1453     case 1:
1454      if (PREAMBLE2 == data) {
1455        _step++;
1456        break;
1457       }
1458      _step = 0;
1459      goto restart;
1460    case 2:
1461      if (sizeof(_buffer)   == data) {
1462        _step++;
1463        _ck_b = _ck_a = data;                  //   reset the checksum accumulators
1464        _payload_counter = 0;
1465      } else   {
1466        _step = 0;                             // reset and wait for a message   of the right class
1467        goto restart;
1468      }
1469      break;
1470      //   Receive message data
1471    case 3:
1472      _buffer.bytes[_payload_counter++] =   data;
1473      _ck_b += (_ck_a += data);
1474      if (_payload_counter == sizeof(_buffer))
1475         _step++;
1476      break;
1477      // Checksum and message processing
1478     case 4:
1479      _step++;
1480      if (_ck_a != data)
1481        _step = 0;
1482       break;
1483    case 5:
1484      _step = 0;
1485      if (_ck_b != data)
1486         break;
1487      f.GPS_FIX                   = ((_buffer.msg.fix_type ==   FIX_3D) || (_buffer.msg.fix_type == FIX_3D_SBAS));
1488      #if defined(MTK_BINARY16)
1489       GPS_coord[LAT]              = _buffer.msg.latitude * 10;    // XXX doc says   *10e7 but device says otherwise
1490      GPS_coord[LON]              = _buffer.msg.longitude   * 10;   // XXX doc says *10e7 but device says otherwise
1491      #endif
1492      #if   defined(MTK_BINARY19)
1493      GPS_coord[LAT]              = _buffer.msg.latitude;          // With 1.9 now we have real 10e7 precision
1494      GPS_coord[LON]              =   _buffer.msg.longitude;
1495      #endif
1496      GPS_altitude                = _buffer.msg.altitude   /100;    // altitude in meter
1497      GPS_speed                   = _buffer.msg.ground_speed;      // in m/s * 100 == in cm/s
1498      GPS_ground_course           = _buffer.msg.ground_course/100;   //in degrees
1499      GPS_numSat                  = _buffer.msg.satellites;
1500       //GPS_time                    = _buffer.msg.utc_time;
1501      //GPS_hdop                   = _buffer.msg.hdop;
1502      parsed = true;
1503  }
1504  return   parsed;
1505}
1506#endif //MTK
1507
1508#endif //GPS SERIAL
1509
1510
1511
1512
1513/**************************************************************************************/
1514/***************                        I2C GPS                     ********************/
1515/**************************************************************************************/
1516#if   defined(I2C_GPS)
1517#define I2C_GPS_ADDRESS               0x20 //7 bits       
1518
1519#define   I2C_GPS_STATUS_00             00    //(Read only)
1520  #define I2C_GPS_STATUS_NEW_DATA        0x01  // New data is available (after every GGA frame)
1521  #define I2C_GPS_STATUS_2DFIX           0x02  // 2dfix achieved
1522  #define I2C_GPS_STATUS_3DFIX          0x04   // 3dfix achieved
1523  #define I2C_GPS_STATUS_NUMSATS        0xF0  // Number of   sats in view
1524#define I2C_GPS_LOCATION              07    // current location   8 byte (lat, lon) int32_t
1525#define I2C_GPS_GROUND_SPEED          31    // GPS   ground speed in m/s*100 (uint16_t)      (Read Only)
1526#define I2C_GPS_ALTITUDE               33    // GPS altitude in meters (uint16_t)           (Read Only)
1527#define   I2C_GPS_GROUND_COURSE         35    // GPS ground course (uint16_t)
1528#define I2C_GPS_TIME                   39    // UTC Time from GPS in hhmmss.sss * 100 (uint32_t)(unneccesary   precision) (Read Only)
1529#define I2C_GPS_SONAR_ALT             239   // Sonar Altitude
1530
1531uint8_t   GPS_NewData(void) {
1532  uint8_t i2c_gps_status;
1533  
1534  i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00);                  //Get status register 
1535  
1536  #if defined(I2C_GPS_SONAR)
1537   i2c_read_reg_to_buf(I2C_GPS_ADDRESS, I2C_GPS_SONAR_ALT, (uint8_t*)&sonarAlt,2);
1538   #endif
1539  
1540  f.GPS_FIX = 0;	
1541  if (i2c_gps_status & I2C_GPS_STATUS_3DFIX)   {                                     //Check is we have a good 3d fix (numsats>5)
1542     f.GPS_FIX = 1;
1543    if (i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) {                                //Check   about new data
1544      GPS_Frame = 1;
1545      if (GPS_update == 1) GPS_update   = 0; else GPS_update = 1; //Blink GPS update
1546      GPS_numSat = i2c_gps_status   >>4; 
1547      i2c_read_reg_to_buf(I2C_GPS_ADDRESS, I2C_GPS_LOCATION,     (uint8_t*)&GPS_coord[LAT],4);
1548       i2c_read_reg_to_buf(I2C_GPS_ADDRESS, I2C_GPS_LOCATION+4,   (uint8_t*)&GPS_coord[LON],4);
1549       // note: the following vars are currently not used in nav code -- avoid retrieving   it to save time
1550      //i2c_read_reg_to_buf(I2C_GPS_ADDRESS, I2C_GPS_GROUND_SPEED,   (uint8_t*)&GPS_speed,2);
1551      //i2c_read_reg_to_buf(I2C_GPS_ADDRESS, I2C_GPS_ALTITUDE,      (uint8_t*)&GPS_altitude,2);
1552      //i2c_read_reg_to_buf(I2C_GPS_ADDRESS,   I2C_GPS_GROUND_COURSE,(uint8_t*)&GPS_ground_course,2);
1553      return 1;
1554    }
1555   }
1556  return 0;
1557}
1558#endif //I2C_GPS
1559
1560
1561
1562#endif // GPS Defined
1563