GPS yaw rate / wind-up...

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
Post Reply
csurf
Posts: 65
Joined: Mon Dec 23, 2013 5:59 am

GPS yaw rate / wind-up...

Post by csurf »

need some advice/feedback on the following.
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...

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: GPS yaw rate / wind-up...

Post by EOSBandi »

Spin speed is governed with your MAG P. value simply lower MAG P to slow down spin speed.

csurf
Posts: 65
Joined: Mon Dec 23, 2013 5:59 am

Re: GPS yaw rate / wind-up...

Post by csurf »

EOSBandi wrote:Spin speed is governed with your MAG P. value simply lower MAG P to slow down spin speed.

ok, thanks for the tip, but I've already starting to make some other changes to the NAV code.

I've implemented the rotation limit that I mentioned above, and am also adding some logic to enable a couple of other features, such as:
- slow heading adjustment during the climb/descent to RTH height (if RTH height & 'nav controls heading' was enabled/defined) which will allow the copter to start correcting it's heading for home direction prior to achieving the RTH height
- slow heading adjustment during the pos hold at RTH arrival if the take-off bearing for RTH was enabled
- a config define to over-ride the infinite pos hold at RTH arrival and enable auto-landing...

Will test these out today and see how they work...
Hopefully this will achieve what I need, and this way I won't have to alter the mag PID's.

Post Reply