Particle Photon with Pozyx

localize.ino

// This #include statement was automatically added by the Particle IDE.
#include "Pozyx/Pozyx.h"
#include "Pozyx/Pozyx_definitions.h"

#define BYTE_TO_BINARY(byte)  \
  (byte & 0x80 ? '1' : '0'), \
  (byte & 0x40 ? '1' : '0'), \
  (byte & 0x20 ? '1' : '0'), \
  (byte & 0x10 ? '1' : '0'), \
  (byte & 0x08 ? '1' : '0'), \
  (byte & 0x04 ? '1' : '0'), \
  (byte & 0x02 ? '1' : '0'), \
  (byte & 0x01 ? '1' : '0')



////////////////////////////////////////////////
/////////////// POZYX PARAMETERS ///////////////
////////////////////////////////////////////////
uint8_t num_anchors = 4;
uint16_t anchors[4] = {
    0x6819, 
    0x6820, 
    0x683E, 
    0x6847
};
int32_t heights[4]  = {1000, 1000, 1000, 1000};

char *version = "v0.0.4, localize";
char whoami_str[80];
char error_msg[40];
char error_code[4];


void setup() {

    Particle.variable("version", version, STRING);
    Particle.variable("whoami", whoami_str, STRING);
    Particle.variable("errormsg", error_msg, STRING);
    Particle.variable("errorcode", error_code, STRING);
    
    // Register the cloud functions
    Particle.function("calibrate", calibrate);
    Particle.function("clear_error", clear_error);

    Serial.begin(115200);
    int status = Pozyx.begin();

    if(status == POZYX_FAILURE){
        sprintf(error_msg, "ERROR: Unable to connect to POZYX shield");
        set_errorcode_variable();
        //delay(100);
        //abort();
    }
    else {
        set_whoami_variable();
        sprintf(error_msg, "");
    }

}

void loop() {
    
}

/*
 * Set the Particle `whoami` variable string.
 */
void set_whoami_variable() {
    // Get board details.
    int whoami;
    int fw_version;
    int hw_version;
    int selftest;
    uint8_t tmp;
    
    Pozyx.getWhoAmI(&tmp, NULL);
    whoami = (int) tmp;

    Pozyx.getFirmwareVersion(&tmp, NULL);
    fw_version = (int) tmp;

    Pozyx.getHardwareVersion(&tmp, NULL);
    hw_version = (int) tmp;

    Pozyx.getSelftest(&tmp, NULL);
    selftest = (int) tmp;

    sprintf(whoami_str, "ParticlePozyx - 0x%02X, FW ver.: v%d.%d, HW ver.: %d, HW type: %d, Selftest: %c%c%c%c%c%c%c%c",
       whoami, (fw_version & 0xf0) >> 4, fw_version & 0x0f, hw_version & 0x1f, (hw_version & 0xe0) >> 5, BYTE_TO_BINARY(selftest));
}

void set_errorcode_variable() {
    uint8_t ecode;
    Pozyx.regRead(POZYX_ERRORCODE, &ecode, 1);
    sprintf(error_code, "0x%02X", (int) ecode);
}

int calibrate(String instr)
{
    // clear all previous devices in the device list.
    Pozyx.clearDevices();
  
    /* int status = Pozyx.doAnchorCalibration(POZYX_2_5D, 20, num_anchors, anchors, heights); */
    int status = Pozyx.doAnchorCalibration(POZYX_2D, 40, 4, anchors, heights);
    
    if (status != POZYX_SUCCESS){
        sprintf(error_msg, "ERROR: calibration - %d", status);
        set_errorcode_variable();
    //abort();
    }
    return status;
}

int clear_error(String instr) {
    sprintf(error_msg, "");
    return 1;
}

// print out the anchor coordinates (also required for the processing sketch)
void set_calibration_variable(){
  uint8_t list_size;
  int status;

  status = Pozyx.getDeviceListSize(&list_size);
  //Serial.print("list size: ");
  //Serial.println(status*list_size);
  
  if(list_size == 0){
    //Serial.println("Calibration failed.");
    //Serial.println(Pozyx.getSystemError());
    return;
  }
  
  uint16_t device_ids[list_size];
  status &= Pozyx.getDeviceIds(device_ids,list_size);
  
  //Serial.println(F("Calibration result:"));
  //Serial.print(F("Anchors found: "));
  //Serial.println(list_size);
  
  coordinates_t anchor_coor;
  for(int i=0; i<list_size; i++)
  {
    
    //Serial.print("ANCHOR,");
    //Serial.print("0x");
    //Serial.print(device_ids[i], HEX);
    //Serial.print(",");    
    status = Pozyx.getDeviceCoordinates(device_ids[i], &anchor_coor);
    //Serial.print(anchor_coor.x);
    //Serial.print(",");
    //Serial.print(anchor_coor.y);
    //Serial.print(",");
    //Serial.println(anchor_coor.z);
  }    
}