Testing new nav code

The new navigation code involves a lot of changes:

• Uses new filter architecture which better separates the algorithm from the architectural code
• New state estimation algorithm to determine position and velocity based on some of the inertial navigation code from Ardupilot
• New VTOL follower architecture that allows more complicated behaviors, such as a multiple stage RTH with a PH at the beginning and end and then landing at home

Selecting and tuning state estimation

1. Set StateFilter.Filter to CompNav
2. Enable the GPS module and make sure the GPS settings (baud rate and NMEA/UBX) match your GPS
3. In the attitude page, perform a 6 point calibration and save the results. Then level your quad.
4. Reboot FC and verify that the CFNavState UAVO is updating (note this step is important in case you have a firmware with the old attitude architecture).
5. You should see attitude in the error condition if you don't have any GPS lock (defined as >= 7 satellites, PDOP <= 3.5). Verify that you can see a GPS lock and the attitude behaves reasonably. You should see the position on the map (or via android) nice and stably. Check that when the board is facing north, it really is facing north (once you have GPS lock)
6. There are only two settings for tuning the inertial filter. The horizontal and vertical tau (AttitudeSettings->HorizontalPositionTau, AttitudeSettings->VerticalPositionTau). I would recommend starting with both of them at 2. Make sure to look at your altitude on the PFD and your position on the map that they are stable and reasonable before taking off. A higher value for the means that it will take longer to listen to the GPS or the Baro.

Vtol Path Follower settings

There are a few settings to be familiar with in order to be adjust the path follower.

• VtolPathFollowerSettings.ThrottleControl - this is the most important setting. Start with this at false which means when you use navigation modes you still have control of throttle. This makes it easy to dump out if something is not going well.
• Some recommend settings:
• VtolPathFollowerSettings.HorizontalPosPI - Kp 0.3, Ki 0, ILimit 0
• VtolPathFollowerSettings.HorizontalVelPI - Kp 3, Ki 0, Kd 0, ILimit 0
• VtolPathFollowerSettings.VelocityPosPI - Kp 0.9, Ki 0.05, ILimit 2
• VtolPathFollowerSettings.VerticalVelPID - Kp 0.2, Ki 0.05, Kd 0, ILimit 0.5

Initial sanity checks:

• Fly around medium aggressively in attitude mode and make sure your attitude behaves reasonably. If you are having issues in regular flight then the next step will not go well.
• Email a log from your tablet to yourself and me / the group. The tablet application always logs while connected. Verify that it says "loaded appropriate UAVO set" each time. You'll know it's right probably cause it will be saying things. To email a log, just go to the logging page and select the most recent log.
• Try replaying your log in the GCS. Make sure it shows you flying in the right place and seems to match your flight reasonably.

Enabling position hold:

• Set one of your flight mode position switches to position hold
• Make sure these modules are running: VtolPathFollower, GPS
• Fly, get the system hovering nicely, and enable PH mode

Enabling RTH

• Set one of your flight mode positions to "ReturnToHome"
• Make sure these modules are running: VtolPathFollower, Geofence, GPS
• It is STRONGLY recommended to enable the GeoFence module before testing this to avoid any potential for flyaway. This will cut motors at 250 m from home in the current defaults.
• Fly, get the system hovering nicely, and enable RTH mode. Expect it to hover in place for 10 seconds, climb to 15 m (if below that), and then proceed to fly home. Once over home it will hover for 10 seconds, and then descend at 0.5 m/s.

Important checks, preflights, gotchas:

• Always keep throttle above minimum level while autonomous or it will automatically disarm
• Must have solid satellite lock. Dropping below 6 satellites or PDOP 3.5 will make it go to random places (android will announce a GPS warning and attitude error when this occurs).
• Absolutely must be in INSOutdoor mode. You can easily have fly away or really bad flight otherwise and currently no alarms block this. #453
• To get a good altitude hold, make sure your accelerometers are well calibrated. It should be quite close to 9.81 when sitting flat. Using the level and zero button will help with this.
• It is RTH not RTL so always remember to set HomeLocation where you want to return to. I recommend not saving the HomeLocation for autonomous testing. Then it will grab the value when it gets the first good lock. Depending on balance of baro and GPS altitude home may not be at altitude zero so when it flies home it might hit the ground! This needs work. Use android to see what your home altitude is before taking off - it should be close to 0. Android will also allow resetting just the altitude if desired.
• Check your velocity actual down before flying. The z-accel bias seems to cause errors in this which means you won't get a good enough vertical velocity estimate.
• Verify tablet info is updating before using any of those modes #456

Testing position hold:

• First you want to test with manual control of the throttle. This keeps you in charge and makes sit easy to dump out to ground if there is an issue. Also altitude control isn't working as well as I'd like yet :(. To enable this in VtolPathFollowerSettings make sure ThrottleControl is set to FALSE.
• Get hovering, position nice and stable and at least a few meters above ground. Flip to position hold mode.
• If that works well set ThrottleControl to TRUE and repeat