コード例 #1
0
ファイル: GPS.cpp プロジェクト: robido/TytoRelayRemote
////////////////////////////////////////////////////////////////////////////////////
// Sets the waypoint to navigate, reset neccessary variables and calculate initial values
//
void GPS_set_next_wp(int32_t* lat, int32_t* lon) {
  GPS_WP[LAT] = *lat;
  GPS_WP[LON] = *lon;
 
  GPS_calc_longitude_scaling(*lat);
  GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);

  nav_bearing = target_bearing;
  GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
  original_target_bearing = target_bearing;
  waypoint_speed_gov = NAV_SPEED_MIN;
}
コード例 #2
0
void GPS_calculateDistanceAndDirectionToHome(void)
{
    if (STATE(GPS_FIX_HOME)) {      // If we don't have home set, do not display anything
        uint32_t dist;
        int32_t dir;
        GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
        GPS_distanceToHome = dist / 100;
        GPS_directionToHome = dir / 100;
    } else {
        GPS_distanceToHome = 0;
        GPS_directionToHome = 0;
    }
}
コード例 #3
0
ファイル: gps.c プロジェクト: tlshen/FUSIONSDK
void GPS_NewData(void) {
	static uint32_t nav_loopTimer;
	uint32_t dist;
	int32_t  dir;
	uint8_t axis;
  int16_t speed;
	uint16_t c = GPSBufferAvailable();
	while (c--) {
		if (GPS_newFrame(GPSBufferRead())) {
			if (GPS_Info.GPS_update == 1) 
				GPS_Info.GPS_update = 0;
			else 
				GPS_Info.GPS_update = 1;
			
			if (fgps.GPS_FIX && GPS_Info.GPS_numSat >= 5) {
				if (!checkArm()) {
					fgps.GPS_FIX_HOME = 0;
				}
				if (!fgps.GPS_FIX_HOME && checkArm()) {
					GPS_reset_home_position();
				}
				//Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
				GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
				for (axis = 0; axis< 2; axis++) {
					GPS_data[axis] = GPS_Info.GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
					GPS_degree[axis] = GPS_data[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t
					// How close we are to a degree line ? its the first three digits from the fractions of degree
					// later we use it to Check if we are close to a degree line, if yes, disable averaging,
					fraction3[axis] = (GPS_data[axis]- GPS_degree[axis]*10000000) / 10000;
					GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
					GPS_filter[axis][GPS_filter_index] = GPS_data[axis] - (GPS_degree[axis]*10000000); 
					GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
					GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
					if ( nav_mode == NAV_MODE_POSHOLD) {      //we use gps averaging only in poshold mode...
						if ( fraction3[axis]>1 && fraction3[axis]<999 ) 
							GPS_Info.GPS_coord[axis] = GPS_filtered[axis];
					}
				}
#endif
				//Time for calculating x,y speed and navigation pids
				dTnav = (float)(getTickCount() - nav_loopTimer)/ 10000.0f;
				nav_loopTimer = getTickCount();
				// prevent runup from bad GPS
				dTnav = min(dTnav, 1.0f);  
				//calculate distance and bearings for gui and other stuff continously - From home to copter
				GPS_distance_cm_bearing(&GPS_Info.GPS_coord[LAT],&GPS_Info.GPS_coord[LON],&GPS_Info.GPS_home[LAT],&GPS_Info.GPS_home[LON],&dist,&dir);
				GPS_Info.GPS_distanceToHome = dist/100;
				GPS_Info.GPS_directionToHome = dir/100;
				
				if (!fgps.GPS_FIX_HOME) {     //If we don't have home set, do not display anything
					GPS_Info.GPS_distanceToHome = 0;
					GPS_Info.GPS_directionToHome = 0;
				}
        //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
        GPS_calc_velocity();        
        if (fgps.GPS_HOLD_MODE || fgps.GPS_HOME_MODE){    //ok we are navigating 
          //do gps nav calculations here, these are common for nav and poshold  
          #if defined(GPS_LEAD_FILTER)
          GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
          GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
          #else
          GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
          GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
          #endif
          switch (nav_mode) {
            case NAV_MODE_POSHOLD: 
              //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
              GPS_calc_poshold();
              break;
            case NAV_MODE_WP:
              speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV);      //slow navigation 
              // use error as the desired rate towards the target
              //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
              GPS_calc_nav_rate(speed);
              //Tail control
              if (NAV_CONTROLS_HEADING) {
                if (NAV_TAIL_FIRST) {
                  magHold = wrap_18000(nav_bearing-18000)/100;
                } else {
                  magHold = nav_bearing/100;
                }
              }
              // Are we there yet ?(within 2 meters of the destination)
              if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){         //if yes switch to poshold mode
                nav_mode = NAV_MODE_POSHOLD;
                if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
              } 
              break;               
          }
        } //end of gps calcs  
      }
    }
  }
}
コード例 #4
0
ファイル: GPS.cpp プロジェクト: robido/TytoRelayRemote
uint8_t GPS_NewData(void) {
  uint8_t axis;
  #if defined(I2C_GPS)
    static uint8_t _i2c_gps_status;

    //Do not use i2c_writereg, since writing a register does not work if an i2c_stop command is issued at the end
    //Still investigating, however with separated i2c_repstart and i2c_write commands works... and did not caused i2c errors on a long term test.

    GPS_numSat = (_i2c_gps_status & 0xf0) >> 4;
    _i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00);                 //Get status register 

    uint8_t *varptr;
    #if defined(I2C_GPS_SONAR)
      i2c_rep_start(I2C_GPS_ADDRESS<<1);
      i2c_write(I2C_GPS_SONAR_ALT);         
      i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

      varptr = (uint8_t *)&sonarAlt;          // altitude (in cm? maybe)
      *varptr++ = i2c_readAck();
      *varptr   = i2c_readNak();
    #endif	
    
    if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) {                                     //Check is we have a good 3d fix (numsats>5)
      f.GPS_FIX = 1;

      if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) {                                //Check about new data
        GPS_Frame = 1;

        i2c_rep_start(I2C_GPS_ADDRESS<<1);
        i2c_write(I2C_GPS_LOCATION);                //Start read from here 2x2 bytes distance and direction
        i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

        varptr = (uint8_t *)&GPS_coord[LAT];        // for latitude displaying
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();

        varptr = (uint8_t *)&GPS_coord[LON];        // for longitude displaying
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readNak();

        i2c_rep_start(I2C_GPS_ADDRESS<<1);
        i2c_write(I2C_GPS_GROUND_SPEED);          
        i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);

        varptr = (uint8_t *)&GPS_speed;          // speed in cm/s for OSD
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();

        varptr = (uint8_t *)&GPS_altitude;       // altitude in meters for OSD
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readAck();	

        varptr = (uint8_t *)&GPS_ground_course;
        *varptr++ = i2c_readAck();
        *varptr   = i2c_readNak();

      } else {
        return 0; 
      }
    } else {
      f.GPS_FIX = 0;
      return 0;
    }
  #endif

  #if defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
    #if defined(GPS_SERIAL)
      uint8_t c = SerialAvailable(GPS_SERIAL);
      if (c==0) return 0;
      while (c--) {
        if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
    #elif defined(GPS_FROM_OSD)
      {
        if(GPS_update & 2) {  // Once second bit of GPS_update is set, indicate new GPS datas is readed from OSD - all in right format.
          GPS_update &= 1;    // We have: GPS_fix(0-2), GPS_numSat(0-15), GPS_coord[LAT & LON](signed, in 1/10 000 000 degres), GPS_altitude(signed, in meters) and GPS_speed(in cm/s)                     
    #endif
          GPS_Frame = 1;
        }
      }
  #endif
  return 1;
}

uint8_t GPS_Compute(void) {
  if (GPS_Frame == 0) return 0; else GPS_Frame = 0;

  if (GPS_update == 1) GPS_update = 0; else GPS_update = 1;
  if (f.GPS_FIX && GPS_numSat >= 5) {
    #if !defined(DONT_RESET_HOME_AT_ARM)
       if (!f.ARMED) {f.GPS_FIX_HOME = 0;}
    #endif
    if (!f.GPS_FIX_HOME && f.ARMED) {
      GPS_reset_home_position();
    }

    //Apply moving average filter to GPS data
    #if defined(GPS_FILTERING)
      GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
      for (uint8_t axis = 0; axis< 2; axis++) {
        GPS_read[axis] = GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
        GPS_degree[axis] = GPS_read[axis] / 10000000;  // get the degree to assure the sum fits to the int32_t
    
        // How close we are to a degree line ? its the first three digits from the fractions of degree
        // later we use it to Check if we are close to a degree line, if yes, disable averaging,
        fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000;
    
        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
        GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000); 
        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
        GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
        if ( nav_mode == NAV_MODE_POSHOLD) {      //we use gps averaging only in poshold mode...
          if ( fraction3[axis]>1 && fraction3[axis]<999 ) GPS_coord[axis] = GPS_filtered[axis];
        }
      }
    #endif

    //dTnav calculation
    //Time for calculating x,y speed and navigation pids
    static uint32_t nav_loopTimer;
    dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
    nav_loopTimer = millis();
    // prevent runup from bad GPS
    dTnav = min(dTnav, 1.0);  

    //calculate distance and bearings for gui and other stuff continously - From home to copter
    uint32_t dist;
    int32_t  dir;
    GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist,&dir);
    GPS_distanceToHome = dist/100;
    GPS_directionToHome = dir/100;

    if (!f.GPS_FIX_HOME) {     //If we don't have home set, do not display anything
      GPS_distanceToHome = 0;
      GPS_directionToHome = 0;
    }

    //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
    GPS_calc_velocity();        

    if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE){    //ok we are navigating 
      //do gps nav calculations here, these are common for nav and poshold  
      #if defined(GPS_LEAD_FILTER)
        GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
        GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
      #else
        GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
        GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
      #endif
      switch (nav_mode) {
        case NAV_MODE_POSHOLD: 
          //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
          GPS_calc_poshold();
          break;
        case NAV_MODE_WP:
          int16_t speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV);      //slow navigation 
          // use error as the desired rate towards the target
          //Desired output is in nav_lat and nav_lon where 1deg inclination is 100 
          GPS_calc_nav_rate(speed);

          //Tail control
          if (NAV_CONTROLS_HEADING) {
            if (NAV_TAIL_FIRST) {
              magHold = wrap_18000(nav_bearing-18000)/100;
            } else {
              magHold = nav_bearing/100;
            }
          }
          // Are we there yet ?(within 2 meters of the destination)
          if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){         //if yes switch to poshold mode
            nav_mode = NAV_MODE_POSHOLD;
            if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
          } 
          break;               
      }
    } //end of gps calcs  
  }
}


void GPS_reset_home_position(void) {
  if (f.GPS_FIX && GPS_numSat >= 5) {
    GPS_home[LAT] = GPS_coord[LAT];
    GPS_home[LON] = GPS_coord[LON];
    GPS_calc_longitude_scaling(GPS_coord[LAT]);  //need an initial value for distance and bearing calc
    nav_takeoff_bearing = att.heading;             //save takeoff heading
    //Set ground altitude
    f.GPS_FIX_HOME = 1;
  }
}

//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void) {
  uint8_t i;
  
  for(i=0;i<2;i++) {
    nav_rated[i] = 0;
    nav[i] = 0;
    reset_PID(&posholdPID[i]);
    reset_PID(&poshold_ratePID[i]);
    reset_PID(&navPID[i]);
    nav_mode = NAV_MODE_NONE;
  }
}

//Get the relevant P I D values and set the PID controllers 
void GPS_set_pids(void) {
  posholdPID_PARAM.kP   = (float)conf.pid[PIDPOS].P8/100.0;
  posholdPID_PARAM.kI   = (float)conf.pid[PIDPOS].I8/100.0;
  posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
  
  poshold_ratePID_PARAM.kP   = (float)conf.pid[PIDPOSR].P8/10.0;
  poshold_ratePID_PARAM.kI   = (float)conf.pid[PIDPOSR].I8/100.0;
  poshold_ratePID_PARAM.kD   = (float)conf.pid[PIDPOSR].D8/1000.0;
  poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
  
  navPID_PARAM.kP   = (float)conf.pid[PIDNAVR].P8/10.0;
  navPID_PARAM.kI   = (float)conf.pid[PIDNAVR].I8/100.0;
  navPID_PARAM.kD   = (float)conf.pid[PIDNAVR].D8/1000.0;
  navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
コード例 #5
0
void onGpsNewData(void)
{
    int axis;
    static uint32_t nav_loopTimer;
    uint16_t speed;


    if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
        return;
    }

    if (!ARMING_FLAG(ARMED))
        DISABLE_STATE(GPS_FIX_HOME);

    if (!STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED))
        GPS_reset_home_position();

    // Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
    GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
    for (axis = 0; axis < 2; axis++) {
        GPS_read[axis] = GPS_coord[axis];               // latest unfiltered data is in GPS_latitude and GPS_longitude
        GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t

        // How close we are to a degree line ? its the first three digits from the fractions of degree
        // later we use it to Check if we are close to a degree line, if yes, disable averaging,
        fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;

        GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
        GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
        GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
        GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
        if (nav_mode == NAV_MODE_POSHOLD) {             // we use gps averaging only in poshold mode...
            if (fraction3[axis] > 1 && fraction3[axis] < 999)
                GPS_coord[axis] = GPS_filtered[axis];
        }
    }
#endif

    //
    // Calculate time delta for navigation loop, range 0-1.0f, in seconds
    //
    // Time for calculating x,y speed and navigation pids
    dTnav = (float)(millis() - nav_loopTimer) / 1000.0f;
    nav_loopTimer = millis();
    // prevent runup from bad GPS
    dTnav = min(dTnav, 1.0f);

    GPS_calculateDistanceAndDirectionToHome();

    // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
    GPS_calc_velocity();

    if (FLIGHT_MODE(GPS_HOLD_MODE) || FLIGHT_MODE(GPS_HOME_MODE)) {
        // we are navigating

        // gps nav calculations, these are common for nav and poshold
        GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
        GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);

        switch (nav_mode) {
        case NAV_MODE_POSHOLD:
            // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
            GPS_calc_poshold();
            break;

        case NAV_MODE_WP:
            speed = GPS_calc_desired_speed(gpsProfile->nav_speed_max, NAV_SLOW_NAV);    // slow navigation
            // use error as the desired rate towards the target
            // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
            GPS_calc_nav_rate(speed);

            // Tail control
            if (gpsProfile->nav_controls_heading) {
                if (NAV_TAIL_FIRST) {
                    magHold = wrap_18000(nav_bearing - 18000) / 100;
                } else {
                    magHold = nav_bearing / 100;
                }
            }
            // Are we there yet ?(within x meters of the destination)
            if ((wp_distance <= gpsProfile->gps_wp_radius) || check_missed_wp()) {      // if yes switch to poshold mode
                nav_mode = NAV_MODE_POSHOLD;
                if (NAV_SET_TAKEOFF_HEADING) {
                    magHold = nav_takeoff_bearing;
                }
            }
            break;
        default:
            break;
        }
    }                   //end of gps calcs
}
コード例 #6
0
ファイル: gps.c プロジェクト: mcu786/baseflight-1
void GPS_NewData(uint16_t c)
{
    uint8_t axis;
    static uint32_t nav_loopTimer;
    uint32_t dist;
    int32_t dir;
    int16_t speed;

    if (GPS_newFrame(c)) {
        if (GPS_update == 1)
            GPS_update = 0;
        else
            GPS_update = 1;
        if (GPS_fix == 1 && GPS_numSat >= 5) {
            if (armed == 0) {
                GPS_fix_home = 0;
            }
            if (GPS_fix_home == 0 && armed) {
                GPS_fix_home = 1;
                GPS_home[LAT] = GPS_coord[LAT];
                GPS_home[LON] = GPS_coord[LON];
                GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
                nav_takeoff_bearing = heading; // save takeoff heading
            }
            // Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
            GPS_filter_index = ++GPS_filter_index % GPS_FILTER_VECTOR_LENGTH;
            for (axis = 0; axis < 2; axis++) {
                GPS_read[axis] = GPS_coord[axis];       // latest unfiltered data is in GPS_latitude and GPS_longitude
                GPS_degree[axis] = GPS_read[axis] / 10000000;   // get the degree to assure the sum fits to the int32_t

                // How close we are to a degree line ? its the first three digits from the fractions of degree
                // later we use it to Check if we are close to a degree line, if yes, disable averaging,
                fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;

                GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
                GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
                GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
                GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
                if (nav_mode == NAV_MODE_POSHOLD) {     //we use gps averaging only in poshold mode...
                    if (fraction3[axis] > 1 && fraction3[axis] < 999)
                        GPS_coord[axis] = GPS_filtered[axis];
                }
            }
#endif
            // dTnav calculation
            // Time for calculating x,y speed and navigation pids
            dTnav = (float) (millis() - nav_loopTimer) / 1000.0f;
            nav_loopTimer = millis();
            // prevent runup from bad GPS
            dTnav = min(dTnav, 1.0f);

            // calculate distance and bearings for gui and other stuff continously - From home to copter
            GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
            GPS_distanceToHome = dist / 100;
            GPS_directionToHome = dir / 100;

            //calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
            GPS_calc_velocity();

            if (GPSModeHold == 1 || GPSModeHome == 1) { // ok we are navigating
                // do gps nav calculations here, these are common for nav and poshold
                GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
                GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);

                switch (nav_mode) {
                case NAV_MODE_POSHOLD:
                    // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
                    GPS_calc_poshold();
                    break;

                case NAV_MODE_WP:
                    speed = GPS_calc_desired_speed(cfg.nav_speed_max, NAV_SLOW_NAV);    //slow navigation
                    // use error as the desired rate towards the target
                    // Desired output is in nav_lat and nav_lon where 1deg inclination is 100
                    GPS_calc_nav_rate(speed);

                    // Tail control
                    if (cfg.nav_controls_heading) {
                        if (NAV_TAIL_FIRST) {
                            magHold = wrap_18000(nav_bearing - 18000) / 100;
                        } else {
                            magHold = nav_bearing / 100;
                        }
                    }
                    // Are we there yet ?(within x meters of the destination)
                    if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp()) {      // if yes switch to poshold mode
                        nav_mode = NAV_MODE_POSHOLD;
                        if (NAV_SET_TAKEOFF_HEADING) {
                            magHold = nav_takeoff_bearing;
                        }
                    }
                    break;
                }
            }                   //end of gps calcs
        }
    }
}
コード例 #7
0
ファイル: AP_GPS.c プロジェクト: BobLiu20/AirPilot
/***********************
函数功能:GPS数据更新,将由串口中断调用,在GPS初始化时传入了
***********************/
static void GPS_NewData(void)
{
	int axis;
	static uint32_t nav_loopTimer;	//记录上一次的循环的时间戳
	uint32_t dist;	//距离
  int32_t dir;	//方向
  int16_t speed;	//速度
	SensorsSet(SENSOR_GPS);	//置位GPS存在标志
	if(GPS_update == 1)
		GPS_update = 0;
	else
		GPS_update = 1;
	if(f.GPS_FIX && GPS_numSat >= 5)//如果数据有效和卫星数大于5
	{
		if(!f.ARMED)	
		{
			f.GPS_FIX_HOME = 0;//在没有解锁状态下,清楚GPS HOME点
		}
		if(!f.GPS_FIX_HOME && f.ARMED)//在没有GPS原点又已经解锁了,则设置原点
		{
			f.GPS_FIX_HOME = 1;//标记有原点
			GPS_home[LAT] = GPS_coord[LAT];//将当前经纬度记录下
			GPS_home[LON] = GPS_coord[LON];
			GPS_calc_longitude_scaling(GPS_coord[LAT]); //得到一个初始值用于计算距离和方位
      nav_takeoff_bearing = heading; // save takeoff heading
			XbeePro_ImportanceInfo = XbeePro_ImportanceInfo | 0x10;//发送HOME点给上位机
		}
		//进行滤波,美其名曰滑动平均滤波
		GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
    for (axis = 0; axis < 2; axis++) //axis=0时为纬度,=1时为经度
		{
			GPS_read[axis] = GPS_coord[axis];       //得到最新的未过滤的经纬度
      GPS_degree[axis] = GPS_read[axis] / 10000000;   //得到经纬度的度

			// How close we are to a degree line ? its the first three digits from the fractions of degree
			// later we use it to Check if we are close to a degree line, if yes, disable averaging,
			fraction3[axis] = (GPS_read[axis] - GPS_degree[axis] * 10000000) / 10000;//得到小数点后三位的分,即0.xxx度的xxx

			GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
			GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis] * 10000000);
			GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
			GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
			if (nav_mode == NAV_MODE_POSHOLD) //我们只在poshold模式下使用GPS平均值		nav_mode == NAV_MODE_POSHOLD
			{
				if (fraction3[axis] > 1 && fraction3[axis] < 999)
					GPS_coord[axis] = GPS_filtered[axis];	//将滤波后的值保存
      }
    }
		//进入计算 x,y 、速度、航向 的PID等
		dTnav = (float) (TimMsGet() - nav_loopTimer) / 1000.0f;//计算两次之间的时间差	  现在单位为s
		nav_loopTimer = TimMsGet();		//记录时间戳  单位为ms
		dTnav = min(dTnav, 1.0f);		//防止GPS出错,产生超长时间差(不大于1S)
		//计算距离、方位给GUI,从原点到灰机的
		GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
    GPS_distanceToHome = dist / 100;	//将单位变为米
    GPS_directionToHome = dir / 100;	//现在单位是度了
		//计算基于GPS的速率,当我们有启动导航
		GPS_calc_velocity();	
		if(f.GPS_HOLD_MODE || f.GPS_HOME_MODE)	//悬停或者返回原点模式,则启动导航哈
		{
			if(GS_Nav)		 //	地面站有新导航任务  GS_Nav标志置一	ReadyForNextWP置一 ReadyToMove = 0; nav_mode = NAV_MODE_POSHOLD;	wp=0;
			{
				if(ReadyForNextWP)			   //已经到达航点,为下一个航点做准备
				{
					if(AutoHeading==0)			//如果不需要自动转向  就直接飞向下一个航点
					{
						ReadyToMove = 1;	  //这样就不会执行下面的转向和等待转向
					}
					if(ReadyToMove)		 //	  如果转向完毕,或不需要转向,则设定下一个航点并开启飞航线模式
					{
						Set_nextwp();	
						nav_mode = NAV_MODE_WP;	
											
						pid_nav_crosstrack._integrator = 0;
						pid_nav_crosstrack._last_input = 0;
						pid_nav_crosstrack._last_derivative = 0;
						
					}
					else				   //执行自动转向
					{
						magHold = GPS_BearingForNextwp(&GPS_coord[LAT], &GPS_coord[LON], &Waypoint[wp][LAT], &Waypoint[wp][LON]);//计算下个点的方位角
						magHold /=100;		//切记切记
					//	if(abs(heading - magHold) < 10 || abs(heading - magHold) > 350)			 //航向调整完再走	   	nav_bearing/100
					//	{
							ReadyToMove = 1;
					//	}
					}
				}
			}
			//GPS导航计算
			GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);//计算直线距离和方位角
			GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);//得到X,Y距离

			switch (nav_mode)
			{
				case NAV_MODE_POSHOLD:	//位置保持模式
						GPS_calc_poshold();
						break;
				case NAV_MODE_WP:
//						if(Waypoint[wp+1][0]==0)
							speed = GPS_calc_desired_speed(NAV_Speed, true);//只有到最后一个点才减速
// 						else
// 							speed = GPS_calc_desired_speed(NAV_Speed, false);	 //  cfg.nav_speed_max

				// use error as the desired rate towards the target
						// 期望的输出是在导航纬度和经度,1度倾向是100

						GPS_calc_nav_rate(speed);

						//方向控制,即调整YAW
						if (AutoHeading) 
						{
							#if (NAV_TAIL_FIRST) 
							magHold = wrap_18000(target_bearing - 18000) / 100;
							#else 
							magHold = target_bearing / 100;			//		 nav_bearing
							#endif
						}
						//判断是否到达目的地(在一个允许的半径内)
						if ((wp_distance <= cfg.gps_wp_radius) || check_missed_wp())   //暂时是2米半径
						{
//							nav_mode = NAV_MODE_POSHOLD;//如果到达或失去目标则进入悬停
							ReadyForNextWP=1;
							ReadyToMove = 1 ;
							if(Waypoint[wp][LAT]==0)			//没有下一个航点了  
							{						
								nav_mode = NAV_MODE_POSHOLD;//如果到达或失去目标则进入悬停
								GPS_hold[LAT] =     GPS_WP[LAT];			 //更新hold点坐标
								GPS_hold[LON] =     GPS_WP[LON];
								XbeePro_ImportanceInfo = XbeePro_ImportanceInfo | 0x01;//发送HOLD点给上位机
								ReadyForNextWP=0;
								GS_Nav = 0;				//没有新航点时 ,保证不更行航点
								if (NAV_SET_TAKEOFF_HEADING)//是否保持与起飞时的方向相同
								{
									magHold = nav_takeoff_bearing;
								}
							}
						}
						else
						{
							ReadyForNextWP = 0;		  //不要更新航点
						}
						break;
			}
		}
	}
}