Add geodetic2ecef function.

README.md

@@ -25,7 +25,7 @@ XYZgeomag uses single precision floating points. It's designed to minimize ram u
 
 ## Using XYZgeomag
 
-Just download [geomag.hpp](https://github.com/nhz2/XYZgeomag/releases/download/v1.0.0/geomag.hpp) and include it.
+Just download [geomag.hpp](https://raw.githubusercontent.com/nhz2/XYZgeomag/master/geomag.hpp) and include it.
 Here is an example Arduino sketch:
 
 ~~~cpp
@@ -49,9 +49,12 @@ void loop() {
   out=geomag::GeoMag(2022.5,in,geomag::WMM2020);
   int endtime=micros();
   int endtimemil=millis();
-  Serial.println(out.x*1E9);
-  Serial.println(out.y*1E9);
-  Serial.println(out.z*1E9);
+  Serial.print(out.x*1E9);
+  Serial.println(" nT x");
+  Serial.print(out.y*1E9);
+  Serial.println(" nT y");
+  Serial.print(out.z*1E9);
+  Serial.println(" nT z");
   Serial.print("time in micro seconds: ");
   Serial.println(endtime-starttime);
   Serial.print("time in milli seconds: ");
@@ -60,6 +63,36 @@ void loop() {
 }
 ~~~
 
+If you have a position in latitude, longitude, and height, 
+you can convert it to geocentric cartesian coordinates 
+with `geodetic2ecef`. Note that `geodetic2ecef` uses 
+single precision floats, so it will only be accurate to about 1 meter:
+~~~cpp
+#include "geomag.hpp"
+void setup() {
+  // put your setup code here, to run once:
+  pinMode(1,INPUT);
+  Serial.begin(9600);
+}
+
+void loop() {
+  // put your main code here, to run repeatedly:
+  int val= digitalRead(1);
+  float lat = val + 43.0f; // latitude in degrees
+  float lon = val + 75.0f; // longitude in degrees
+  float height = val + 305; // height above WGS84 ellipsoid in meters
+  geomag::Vector in = geomag::geodetic2ecef(lat,lon,height);
+  geomag::Vector out = geomag::GeoMag(2022.5,in,geomag::WMM2020);
+  Serial.print(out.x*1E9);
+  Serial.println(" nT x");
+  Serial.print(out.y*1E9);
+  Serial.println(" nT y");
+  Serial.print(out.z*1E9);
+  Serial.println(" nT z");
+  delay(2000);
+}
+~~~
+
 
 
 ## Adding New Coefficents

geomag.hpp

@@ -80,6 +80,34 @@ typedef struct {
     float z;
 } Vector;
 
+
+/** Return the position in International Terrestrial Reference System coordinates, units meters.
+Using the WGS 84 ellipsoid and the algorithm from https://geographiclib.sourceforge.io/
+ INPUT:
+    lat: Geodetic latitude in degrees, -90 at the south pole, 90 at the north pole.
+    lon: Geodetic longitude in degrees.
+    h: Height above the WGS 84 ellipsoid in meters.
+**/
+inline Vector geodetic2ecef(float lat, float lon, float h){
+    // Convert to radians
+    float phi = lat*((float)(M_PI/180.0));
+    float lam = lon*((float)(M_PI/180.0));
+    // WGS 84 constants
+    const float a = 6378137;
+    // const float f = 1.0/298.257223563;
+    const float e2 = 0.0066943799901413165;//f*(2-f);
+    const float e2m = 0.9933056200098587;//(1-f)*(1-f);
+    float sphi = sinf(phi);
+    float cphi = cosf(phi);
+    float slam = sinf(lam);
+    float clam = cosf(lam);
+    float n = a/sqrtf(1.0f - e2*(sphi*sphi));
+    float z = (e2m*n + h) * sphi;
+    float r = (n + h) * cphi;
+    return {r*clam, r*slam, z};
+}
+
+
 /** Return the magnetic field in International Terrestrial Reference System coordinates, units Tesla.
  INPUT:
     position_itrs(Above the surface of earth): The location where the field is predicted, units m.