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
}
}
}
}
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;
}
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
}
}
}
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
}
/***********************
函数功能：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;
}
}
}
}
