A PX4 based camera pointer

latlon.go 1.6KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253
  1. // Copyright 2017 Google Inc.
  2. //
  3. // Licensed under the Apache License, Version 2.0 (the "License");
  4. // you may not use this file except in compliance with the License.
  5. // You may obtain a copy of the License at
  6. //
  7. // http://www.apache.org/licenses/LICENSE-2.0
  8. //
  9. // Unless required by applicable law or agreed to in writing, software
  10. // distributed under the License is distributed on an "AS IS" BASIS,
  11. // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  12. // See the License for the specific language governing permissions and
  13. // limitations under the License.
  14. //
  15. package pipoint
  16. import (
  17. "math"
  18. )
  19. const (
  20. m1 = 111132.92 // latitude calculation term 1
  21. m2 = -559.82 // latitude calculation term 2
  22. m3 = 1.175 // latitude calculation term 3
  23. m4 = -0.0023 // latitude calculation term 4
  24. p1 = 111412.84 // longitude calculation term 1
  25. p2 = -93.5 // longitude calculation term 2
  26. p3 = 0.118 // longitude calculation term 3
  27. )
  28. // LatLength returns the length of a line of latitude at the given
  29. // latitude. Input is in rad, output in m.
  30. func LatLength(lat float64) float64 {
  31. return m1 + m2*math.Cos(2*lat) + m3*math.Cos(4*lat)
  32. }
  33. // LonLength returns the length of a line of longitude at the given
  34. // latitude. Input is in rad, output in m.
  35. func LonLength(lat float64) float64 {
  36. return p1*math.Cos(lat) + p2*math.Cos(3*lat) + p3*math.Cos(5*lat)
  37. }
  38. // AsRad converts degrees to radians.
  39. func AsRad(deg float64) float64 {
  40. return deg * (math.Pi / 180)
  41. }
  42. // AsDeg converts radians to degrees.
  43. func AsDeg(rad float64) float64 {
  44. return rad * (180 / math.Pi)
  45. }