I'm running EOSBandi's latest 2.3 nav version. During testing of the NAV functions (RTH, NAV missions, etc), I've found that the changes in heading are very abrupt. Within the GPS code, I saw the following...
Code: Select all
//GPS.cpp @ line 828
void GPS_adjust_heading()
{
//TODO: Add slow windup for large heading change
//This controls the heading
if (f.GPS_head_set) // We have seen a SET_POI or a SET_HEADING command
{
if (GPS_poi[LAT] == 0)
magHold = wrap_18000((GPS_directionToPoi*100)-18000)/100;
else
{
GPS_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&GPS_directionToPoi);
GPS_distance_cm(&GPS_coord[LAT],&GPS_coord[LON],&GPS_poi[LAT],&GPS_poi[LON],&wp_distance);
magHold = GPS_directionToPoi /100;
}
}
else // heading controlled by the standard defines
if (GPS_conf.nav_controls_heading)
{
if (GPS_conf.nav_tail_first)
{
magHold = wrap_18000(target_bearing-18000)/100;
}
else
{
magHold = wrap_18000(target_bearing)/100;
}
}
}
the above code mentions a pending modification for adding a 'slow wind-up' for large heading changes... I'm considering doing the following and would like some feedback
Code: Select all
//config.h add new define to set max nav yaw rate
#define NAV_MAX_YAW_ANGLE 45 //max yaw correction angle in degrees, zero to disable
//types.h
typedef struct{
...
uint8_t nav_max_yaw_angle;
...
} gps_conf_struct;
//EEPROM.cpp @ line ~298
GPS_conf.nav_max_yaw_angle = NAV_MAX_YAW_ANGLE;
GPS.c @ line 855
void GPS_adjust_heading()
{
//TODO: Add slow windup for large heading change
//This controls the heading
if (f.GPS_head_set) // We have seen a SET_POI or a SET_HEADING command
...
if (GPS_conf.nav_controls_heading)
{
if (GPS_conf.nav_tail_first)
{
magHold = wrap_18000(target_bearing-18000)/100;
}
else
{
magHold = wrap_18000(target_bearing)/100;
}
}
//CSURF: her'es my proposed change, constrain resultant magHold value within the predefined max yaw angle range
if (GPS_conf.nav_max_yaw_angle > 0)
magHold=constrain(magHold, -GPS_conf.nav_max_yaw_angle, GPS_conf.nav_max_yaw_angle);
}
Basically, I'd like to try to limit the speed at which the vehicle will spin/yaw around due to a large calculated heading error when in a GPS NAV mode (RTH or a NAV mission). My idea is that placing an upper limit on the angle error will slow down the spin and result in a smoother heading correction. However, I'm not sure about this, as this is my first attempt at this type of code, so I'd appreciate some feedback & assistance on this... thanks...