Haversine formula

Posted below is my code, I was wondering how i could implement the haversine formula to calculate the distance between two close coordinates. data is coming in from an array from another device and i was going to use that for one of the coordinates and then the other coordinates i would insert myself.

Please Help!

#include <esp_now.h>
#include <WiFi.h>

#define CHANNEL 1

// Init ESP Now with fallback
void InitESPNow() {
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  else {
    Serial.println("ESPNow Init Failed");

// config AP SSID
void configDeviceAP() {
  char* SSID = "Slave_1";
  bool result = WiFi.softAP(SSID, "Slave_1_Password", CHANNEL, 0);
  if (!result) {
    Serial.println("AP Config failed.");
  } else {
    Serial.println("AP Config Success. Broadcasting with AP: " + String(SSID));

void setup() {

  Serial.println("ESPNow/Basic/Slave Example");
  //Set device in AP mode to begin with

  Serial.print("AP MAC: "); Serial.println(WiFi.softAPmacAddress());


void OnDataRecv(const uint8_t *mac_addr,const uint8_t* data, int data_len) {
  char macStr[18];
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
  mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);
  if(data_len != 2*sizeof(float))
    Serial.println("I haven't received 8 bytes of gps data.");
  const float* gpsData = reinterpret_cast<const float*>(data);
  Serial.print("Latitude: ");
  Serial.print("Longitude: ");
  Serial.println("Distance: ");

void loop() {


C permits you to define your own functions. The haversine is defined by a sine function, and the sine function is readily available in C, so it would be easy to write your own haversine function.

The distance between two points on the surface of a sphere can be expressed by the haversine function and other trigonometric functions that are available in C, so, YES, the haversine formula could be implemented.

If you don't want to do it yourself, head over to "Gigs and Collaborations" and bring your wallet.

Sounds good thanks

Navigational basics (distance, bearing, etc.): http://www.movable-type.co.uk/scripts/latlong.html

For short distances the equirectangular approximation works as well or better than the haversine forumula.