Remove repeated code in cgains generation script.

Signed-off-by: Dale Lukas Peterson <hazelnusse@gmail.com>
Change controller gains lookup.

Return the ControllerGains lower and upper bounds instead of nearest.

Signed-off-by: Dale Lukas Peterson <hazelnusse@gmail.com>
Updated cgains and fixed YawRateController accordingly.