diff --git a/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino b/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino
index ec822deef2..0b004b99a3 100644
--- a/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino
+++ b/Tools/Pozyx/IndoorLoiter/IndoorLoiter.ino
@@ -1,120 +1,155 @@
-#include <Time.h>
-#include <TimeLib.h>
-
 #include <Pozyx.h>
 #include <Pozyx_definitions.h>
 
-/*//#include <mavlink.h>*/
-#include "C:\Users\rmackay9\Documents\GitHub\ardupilot\Build.ArduCopter\libraries\GCS_MAVLink\include\mavlink\v2.0\common\mavlink.h"
 #include <SoftwareSerial.h>
 #include <Wire.h>
-/*#include <Time.h> // download from https://github.com/PaulStoffregen/Time */
-//#include "C:\Users\rmackay9\Documents\GitHub\Time\Time.h"
 
 ////////////////// Pozyx Prams //////////////////////////////
+#define CONFIG_TX_GAIN 33.5f
 
 #define NUM_ANCHORS 4
 // the network id of the anchors: change these to the network ids of your anchors.
-uint16_t anchors[4] = { 0x601C, // (0,0)
-                        0x6020, // x-axis
-                        0x6057, // y-axis
-                        0x605E};     
+uint16_t anchor_id[4] = { 0x601C, // (0,0)
+                          0x6020, // x-axis
+                          0x6057, // y-axis
+                          0x605E};     
 
 // only required for manual anchor calibration. 
 // Please change this to the coordinates measured for the anchors
-int32_t anchors_x[NUM_ANCHORS] = {0,    18600, 0,     18600};    // anchor x-coorindates in mm (horizontal)
-int32_t anchors_y[NUM_ANCHORS] = {0,    0,     10000, 10000};    // anchor y-coordinates in mm (vertical)
-int32_t heights[NUM_ANCHORS] =   {1420, 0,     0,     1450};     // anchor z-coordinates in mm
-
-////////////////// MAVLINK Prams //////////////////////////////
-#define LATITUDE_BASE (36.324187 * 1.0e7)
-#define LONGITUDE_BASE (138.639212 * 1.0e7)
-uint8_t buf[MAVLINK_MSG_ID_GPS_INPUT_LEN];
-int32_t latitude = 0;
-int32_t longitude = 0;
+int32_t anchors_x[NUM_ANCHORS] = {0,     10000, 0,     10000};    // anchor x-coorindates in mm (horizontal)
+int32_t anchors_y[NUM_ANCHORS] = {0,     0,     10000, 10000};    // anchor y-coordinates in mm (vertical)
+int32_t heights[NUM_ANCHORS] =   {-1200, -1200, -1200, -1200};    // anchor z-coordinates in mm (1.2m above vehicle's starting altitude)
 
 // RX TX serial for flight controller ex) Pixhawk
 // https://github.com/PaulStoffregen/AltSoftSerial
-
 SoftwareSerial fcboardSerial(10, 11); // rx, tx
 
+#define MSG_HEADER          0x01
+#define MSGID_BEACON_CONFIG 0x02
+#define MSGID_BEACON_DIST   0x03
+#define MSGID_POSITION      0x04
+
+// structure for messages uploaded to ardupilot
+union beacon_config_msg {
+    struct {
+        uint8_t beacon_id;
+        uint8_t beacon_count;
+        int32_t x;
+        int32_t y;
+        int32_t z;
+    } info;
+    uint8_t buf[14];
+};
+union beacon_distance_msg {
+    struct {
+        uint8_t beacon_id;
+        uint32_t distance;
+    } info;
+    uint8_t buf[5];
+};
+union vehicle_position_msg {
+    struct {
+        int32_t x;
+        int32_t y;
+        int32_t z;
+        int16_t position_error;
+    } info;
+    uint8_t buf[14];
+};
 ////////////////////////////////////////////////
 
 void setup()
 {
-  Serial.begin(115200);
-  fcboardSerial.begin(57600);
-  
-  if (Pozyx.begin() == POZYX_FAILURE) {
-    Serial.println(("ERR: shield"));
-    delay(100);
-    abort();
-  }
-  
-  Serial.println(("V1.0"));
+    Serial.begin(115200);
+    fcboardSerial.begin(115200);
 
-  // clear all previous devices in the device list
-  Pozyx.clearDevices();
-     
-  //int status = Pozyx.doAnchorCalibration(POZYX_2_5D, 10, num_anchors, anchors, heights);
-  /*int status = Pozyx.doAnchorCalibration(POZYX_2D, 10, num_anchors, anchors, heights);
-  if (status != POZYX_SUCCESS) {
-    Serial.println(status);
-    Serial.println(("ERROR: calibration"));
-    Serial.println(("Reset required"));
-    delay(100);
-    abort();
-  }*/
-  
-  // if the automatic anchor calibration is unsuccessful, try manually setting the anchor coordinates.
-  // fot this, you must update the arrays anchors_x, anchors_y and heights above
-  // comment out the doAnchorCalibration block and the if-statement above if you are using manual mode
-  SetAnchorsManual();
+    if (Pozyx.begin() == POZYX_FAILURE) {
+        Serial.println(("ERR: shield"));
+        delay(100);
+        abort();
+    }
 
-  printCalibrationResult();
-  Serial.println(("Waiting.."));
-  delay(5000);
+    Serial.println(("V1.0"));
 
-  Serial.println(("Starting: "));
+    // clear all previous devices in the device list
+    Pozyx.clearDevices();
+
+    // configure beacons
+    while (!configure_beacons()) {
+        delay(1000);
+    }
+
+    // if the automatic anchor calibration is unsuccessful, try manually setting the anchor coordinates.
+    // fot this, you must update the arrays anchors_x, anchors_y and heights above
+    // comment out the doAnchorCalibration block and the if-statement above if you are using manual mode
+    SetAnchorsManual();
+
+    print_anchor_coordinates();
+
+    Serial.println(("Waiting.."));
+    delay(5000);
+
+    Serial.println(("Starting: "));
 }
 
 void loop()
-{  
-  coordinates_t position;
-  
-  int status = Pozyx.doPositioning(&position, POZYX_2_5D, 1000);
-  //int status = Pozyx.doPositioning(&position);
-  
-  if (status == POZYX_SUCCESS) {
-    // print out the result
-    printCoordinates(position);
+{
+    static uint32_t loop_start = 0;
+    static uint8_t stage = 0;   // 0 = initialisation, 1 = normal flight
+    static uint16_t beacon_sent_count = 0;
+    static uint32_t beacon_sent_time = 0;
 
-    // send GPS MAVLINK message
-    SendGPSMAVLinkMessage(position);
-  } else {
-    Serial.println("fail");
-  }
+    // initialise start time
+    if (loop_start == 0) {
+        loop_start = millis();
+    }
+
+    // advance to normal flight stage after 1min
+    if (stage == 0) {
+        uint32_t time_diff =  (millis() - loop_start);
+        if (time_diff > 60000) {
+            stage = 1;
+            Serial.println("Stage1");
+        }
+    }
+
+    // slow down counter
+    static uint8_t counter = 0;
+    counter++;
+    if (counter >= 20) {
+        counter = 0;
+    }
+
+    // during stage 0 (init) send position and beacon config as quickly as possible
+    // during stage 1 send about every 2 seconds
+    if (stage == 0 || counter == 0) {
+        send_beacon_config();
+        get_position();
+        if (beacon_sent_count > 0 && beacon_sent_time != 0) {
+            uint32_t time_diff = millis() - beacon_sent_time;
+            float hz = (float)beacon_sent_count / (time_diff / 1000.0f);
+            Serial.print("Beacon hz:");
+            Serial.println(hz);
+        }
+        beacon_sent_count = 0;
+        beacon_sent_time = millis();
+    }
+
+    // send beacon distances
+    get_ranges();
+    beacon_sent_count++;
 }
 
-// function to print the coordinates to the serial monitor
-void printCoordinates(coordinates_t coor)
-{  
-  Serial.print("x:");
-  Serial.print(coor.x);
-  print_tab();
-  Serial.print("y:");
-  Serial.print(coor.y);
-  print_tab();
-  Serial.print("z:");
-  Serial.print(coor.z);
-  print_tab();
-  Serial.print("lat:");
-  Serial.print(latitude);
-  print_tab();
-  Serial.print("lng:");
-  Serial.print(longitude);
-  print_tab();
-  Serial.println(); 
+uint32_t time_start_ms;
+void timer_start()
+{
+    time_start_ms = millis();
+}
+void timer_end()
+{
+    uint32_t time_diff = millis() - time_start_ms;
+    Serial.print("ms:");
+    Serial.println(time_diff);
 }
 
 void print_comma()
@@ -127,40 +162,169 @@ void print_tab()
     Serial.print("\t");
 }
 
-#define LOCATION_SCALING_FACTOR_INV_MM 0.08983204953368922f
-#define DEG_TO_RAD      (M_PI / 180.0f)
-
-float longitude_scale(uint32_t lat)
+// set a tag or anchor's gain
+//   set tag_id to zero to set local device's gain
+//   returns true on success
+bool set_device_gain(uint16_t dev_id, float gain)
 {
-    static int32_t last_lat = 0;
-    static float scale = 1.0;
-    if (labs(last_lat - lat) < 100000) {
-        // we are within 0.01 degrees (about 1km) of the
-        // previous latitude. We can avoid the cos() and return
-        // the same scale factor.
-        return scale;
+    float tx_power = -1;
+
+    // get/set transmit power of tag
+    bool gain_ok = false;
+    uint8_t retry = 0;
+    while (!gain_ok && retry < 5) {
+        if (Pozyx.getTxPower(&tx_power, dev_id) == POZYX_SUCCESS) {
+            if (tx_power != gain) {
+                Pozyx.setTxPower(CONFIG_TX_GAIN, dev_id);
+            } else {
+                gain_ok = true;
+            }
+        }
+        retry++;
     }
-    scale = cosf(lat * 1.0e-7f * DEG_TO_RAD);
-    if (scale < 0.01f) {
-      scale = 0.01f;
+
+    // display final gain
+    Serial.print("Dev ");
+    Serial.print(dev_id, HEX);
+    Serial.print(" gain ");
+    if (tx_power > 0) {
+        Serial.print(tx_power);
+    } else {
+        Serial.print("unknown");
     }
-    if (scale > 1.0f) {
-      scale = 1.0f;
-    }
-    last_lat = lat;
-    return scale;
+    Serial.print(" (retry ");
+    Serial.print(retry);
+    Serial.print(")");
+    Serial.println();
+
+    return gain_ok;
 }
 
-void location_offset(int32_t &lat, int32_t &lng, int32_t offset_north_mm, int32_t offset_east_mm)
+// performs repeated calls to get reliable distance between devices
+bool get_remote_range(uint16_t dev1, uint16_t dev2, int32_t& range)
 {
-    int32_t dlat = offset_north_mm * LOCATION_SCALING_FACTOR_INV_MM;
-    int32_t dlng = (offset_east_mm * LOCATION_SCALING_FACTOR_INV_MM) / longitude_scale(lat);
-    lat += dlat;
-    lng += dlng;
+    // set distances between tags
+    uint32_t range_tot = 0;
+    uint16_t count = 0;
+    device_range_t dev_range;
+    for (uint8_t i=0; i <= 10; i++) {
+        // origin to 1st
+        if (Pozyx.doRemoteRanging(dev1, dev2, &dev_range) == POZYX_SUCCESS) {
+            range_tot += dev_range.distance;
+            count++;
+        }
+        if (Pozyx.doRemoteRanging(dev2, dev1, &dev_range) == POZYX_SUCCESS) {
+            range_tot += dev_range.distance;
+            count++;
+        }
+    }
+    // success if at least 5 successful ranges were retrieved
+    if (count > 5) {
+        range = range_tot / count;
+        return true;
+    }
+    return false;
 }
 
-// print out the anchor coordinates (also required for the processing sketch)
-void printCalibrationResult()
+void print_failed_to_range(uint16_t dev1, uint16_t dev2)
+{  
+    Serial.print("ranging fail ");
+    Serial.print(dev1,HEX);
+    Serial.print(" to ");
+    Serial.println(dev2,HEX);
+}
+
+void set_beacon_position(uint8_t index, int32_t x_mm, int32_t y_mm, int32_t z_mm)
+{
+    anchors_x[index] = x_mm;
+    anchors_y[index] = y_mm;
+    heights[index] = z_mm;
+}
+
+// configure beacons
+bool configure_beacons()
+{
+    bool configured_ok = true;
+
+    // get/set transmit power of tag
+    if (!set_device_gain(0, CONFIG_TX_GAIN)) {
+        configured_ok = false;
+    }
+
+    // set transmit power of beacons    
+    for (uint8_t i=0; i < NUM_ANCHORS; i++) {
+        if (!set_device_gain(anchor_id[i], CONFIG_TX_GAIN)) {
+            configured_ok = false;
+        }
+    }
+
+    // set distances between tags
+    int32_t x_range = 0, y_range = 0;
+    // origin to x-axis (i.e. bottom right)
+    if (get_remote_range(anchor_id[0], anchor_id[1], x_range)) {
+        set_beacon_position(1, x_range, 0, heights[1]);
+    } else {
+        print_failed_to_range(anchor_id[0], anchor_id[1]);
+        configured_ok = false;
+    }
+    // origin to y-axis (i.e. top left)
+    if (get_remote_range(anchor_id[0], anchor_id[2], y_range)) {
+        set_beacon_position(2, 0, y_range, heights[2]);
+    } else {
+        print_failed_to_range(anchor_id[0], anchor_id[2]);
+        configured_ok = false;
+    }
+    // top right
+    if (x_range != 0 && y_range != 0) {
+        set_beacon_position(3, x_range, y_range, heights[3]);
+    } else {
+        Serial.println("beacons too close");
+        configured_ok = false;
+    }
+
+    if (configured_ok) {
+        Serial.println("Beacon Configuration complete");
+    } else {
+        Serial.println("Beacon Configuration failed!");
+    }
+
+    return configured_ok;
+}
+
+// function to manually set the anchor coordinates
+void SetAnchorsManual()
+{
+    for (uint8_t i=0; i<NUM_ANCHORS; i++) {
+        device_coordinates_t anchor;
+        anchor.network_id = anchor_id[i];
+        anchor.flag = 0x1; 
+        anchor.pos.x = anchors_x[i];
+        anchor.pos.y = anchors_y[i];
+        anchor.pos.z = heights[i];
+        Pozyx.addDevice(anchor);
+    }
+}
+
+// print coordinates to the serial monitor
+void print_coordinates(coordinates_t coor, pos_error_t pos_error)
+{  
+    Serial.print("Pos x:");
+    Serial.print(coor.x);
+    print_tab();
+    Serial.print("y:");
+    Serial.print(coor.y);
+    print_tab();
+    Serial.print("z:");
+    Serial.print(coor.z);
+    Serial.print(" err x:");
+    Serial.print(pos_error.x);
+    Serial.print(" y:");
+    Serial.print(pos_error.y);
+    Serial.println(); 
+}
+
+// print out the anchor coordinates
+void print_anchor_coordinates()
 {
   uint8_t list_size;
   int status;
@@ -168,20 +332,21 @@ void printCalibrationResult()
   status = Pozyx.getDeviceListSize(&list_size);
   Serial.print("list: ");
   Serial.println(status*list_size);
-  
+
+  // print error if no anchors are setup
   if (list_size == 0) {
-    Serial.println("Cal fail.");
+    Serial.println("No Anchors");
     Serial.println(Pozyx.getSystemError());
     return;
   }
-  
+
+  // retrieve anchor information
   uint16_t device_ids[list_size];
   status &= Pozyx.getDeviceIds(device_ids,list_size);
-  
-  Serial.println(("Cal:"));
-  Serial.print(("Anchors found: "));
+
+  Serial.print("Anchors found: ");
   Serial.println(list_size);
-  
+
   coordinates_t anchor_coor;
   
   for (int i=0; i<list_size; i++) {
@@ -194,143 +359,116 @@ void printCalibrationResult()
     Serial.print(anchor_coor.y);
     print_comma();
     Serial.println(anchor_coor.z);
-    
   }    
 }
 
-// function to manually set the anchor coordinates
-void SetAnchorsManual()
+// get ranges for each anchor
+void get_ranges()
 {
-  int i=0;
-  
-  for (i=0; i<NUM_ANCHORS; i++) {
-   device_coordinates_t anchor;
-   anchor.network_id = anchors[i];
-   anchor.flag = 0x1; 
-   anchor.pos.x = anchors_x[i];
-   anchor.pos.y = anchors_y[i];
-   anchor.pos.z = heights[i];
-   Pozyx.addDevice(anchor);
- }
-}
-
-// GPS MAVLink message using Pozyx potision
-void SendGPSMAVLinkMessage(coordinates_t position)
-{
-  // Initialize the required buffers 
-  mavlink_message_t msg; 
-  
-  /**
- * @brief Pack a gps_input message
- * @param system_id ID of this system
- * @param component_id ID of this component (e.g. 200 for IMU)
- * @param msg The MAVLink message to compress the data into
- *
- * @param time_usec Timestamp (micros since boot or Unix epoch)
- * @param gps_id ID of the GPS for multiple GPS inputs
- * @param ignore_flags Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum).  All other fields must be provided.
- * @param time_week_ms GPS time (milliseconds from start of GPS week)
- * @param time_week GPS week number
- * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
- * @param lat Latitude (WGS84), in degrees * 1E7
- * @param lon Longitude (WGS84), in degrees * 1E7
- * @param alt Altitude (AMSL, not WGS84), in m (positive for up)
- * @param hdop GPS HDOP horizontal dilution of position in m
- * @param vdop GPS VDOP vertical dilution of position in m
- * @param vn GPS velocity in m/s in NORTH direction in earth-fixed NED frame
- * @param ve GPS velocity in m/s in EAST direction in earth-fixed NED frame
- * @param vd GPS velocity in m/s in DOWN direction in earth-fixed NED frame
- * @param speed_accuracy GPS speed accuracy in m/s
- * @param horiz_accuracy GPS horizontal accuracy in m
- * @param vert_accuracy GPS vertical accuracy in m
- * @param satellites_visible Number of satellites visible.
- * @return length of the message in bytes (excluding serial stream start sign)
- */
- 
-  uint16_t ignore_flags = GPS_INPUT_IGNORE_FLAG_VEL_HORIZ|
-                          GPS_INPUT_IGNORE_FLAG_VEL_VERT|
-                          GPS_INPUT_IGNORE_FLAG_SPEED_ACCURACY|
-                          GPS_INPUT_IGNORE_FLAG_HORIZONTAL_ACCURACY|
-                          GPS_INPUT_IGNORE_FLAG_VERTICAL_ACCURACY;
-  uint32_t time_week_ms = 0;
-  uint16_t time_week = 0;
-
-  make_gps_time(time_week_ms, time_week);
-
-  // adjust position
-  latitude = LATITUDE_BASE;
-  longitude = LONGITUDE_BASE;
-  location_offset(latitude, longitude, position.y, position.x);
-
-  uint16_t len = mavlink_msg_gps_input_pack(
-                    1,
-                    0,
-                    &msg,
-                    micros(),       // time_usec,
-                    0,              // gps_id,
-                    ignore_flags,
-                    time_week_ms,   // time_week_ms,
-                    time_week,      // time_week,
-                    3,              // fix_type,
-                    latitude,       // latitude,
-                    longitude,      // longitude,
-                    10,             // altitude,
-                    1.0f,           // hdop,
-                    1.0f,           // vdop,
-                    0.0f,           // vn
-                    0.0f,           // ve
-                    0.0f,           // vd
-                    0.0f,           // speed_accuracy
-                    0.0f,           // horiz_accuracy
-                    0.0f,           // vert_accuracy,
-                    14              // satellites_visible
-                    );
-
-  // Copy the message to send buffer 
-  len = mavlink_msg_to_send_buffer(buf, &msg);
-  
-  // Send message
-  fcboardSerial.write(buf, len);
-
-}
-
-// calculate GPS time
-// based ardupilot/libraries/AP_GPS/GPS_Backend.cpp
-void make_gps_time(uint32_t &time_week_ms, uint16_t &time_week)
-{
-    uint8_t year, mon, day, hour, min, sec;
-    uint16_t msec;
-    time_t now_time = now();
-
-    year = ::year(now_time);
-    mon  = ::month(now_time);
-    day  = ::day(now_time);
-
-    uint32_t v = millis();
-    msec = v % 1000; v /= 1000;
-    sec  = v % 100; v /= 100;
-    min  = v % 100; v /= 100;
-    hour = v % 100; v /= 100;
-
-    int8_t rmon = mon - 2;
-    if (0 >= rmon) {    
-        rmon += 12;
-        year -= 1;
+    // get range for each anchor
+    device_range_t range;
+    bool success = false;
+    for (uint8_t i=0; i<NUM_ANCHORS; i++) {
+        if (Pozyx.doRanging(anchor_id[i], &range) == POZYX_SUCCESS) {
+            // send info to ardupilot
+            send_beacon_distance(i, range.distance);
+            success = true;
+        }
     }
 
-    // get time in seconds since unix epoch
-    uint32_t ret = (year/4) - 15 + 367*rmon/12 + day;
-    ret += year*365 + 10501;
-    ret = ret*24 + hour;
-    ret = ret*60 + min;
-    ret = ret*60 + sec;
-
-    // convert to time since GPS epoch
-    ret -= 272764785UL;
-
-    // get GPS week and time
-    time_week = ret / (7*86400UL);
-    time_week_ms = (ret % (7*86400UL)) * 1000;
-    time_week_ms += msec;
+    // display errors
+    if (!success) {
+        Serial.println("failed to get any ranges");
+    }
 }
 
+// get position of tag
+void get_position()
+{
+    coordinates_t position;
+    pos_error_t pos_error;
+
+    //if (Pozyx.doPositioning(&position, POZYX_2_5D, 0) == POZYX_SUCCESS) {
+    if (Pozyx.doPositioning(&position, POZYX_3D, 0, 0x00) == POZYX_SUCCESS) {
+        if (Pozyx.getPositionError(&pos_error) == POZYX_SUCCESS) {
+            // display position
+            print_coordinates(position, pos_error);
+            // send to ardupilot
+            send_vehicle_position(position, pos_error);
+        }
+    } else {
+        // display errors
+        Serial.println("failed to calc position");
+    }
+}
+
+// send all beacon config to ardupilot
+void send_beacon_config()
+{
+    beacon_config_msg msg;
+    msg.info.beacon_count = NUM_ANCHORS;
+    for (uint8_t i=0; i<NUM_ANCHORS; i++) {
+        msg.info.beacon_id = i;
+        msg.info.x = anchors_x[i];
+        msg.info.y = anchors_y[i];
+        msg.info.z = heights[i];
+        send_message(MSGID_BEACON_CONFIG, sizeof(msg.buf), msg.buf);
+    }
+    Serial.println("Sent anchor info");
+}
+
+// send a beacon's distance to ardupilot
+void send_beacon_distance(uint8_t beacon_id, uint32_t distance_mm)
+{
+    beacon_distance_msg msg;
+    msg.info.beacon_id = beacon_id;
+    msg.info.distance = distance_mm;
+    send_message(MSGID_BEACON_DIST, sizeof(msg.buf), msg.buf);
+}
+
+// send vehicle's position to ardupilot
+void send_vehicle_position(coordinates_t& position, pos_error_t& pos_error)
+{
+    vehicle_position_msg msg;
+
+    // sanity check position
+    if (position.x == 0 || position.y == 0) {
+        return;
+    }
+
+    msg.info.x = position.x;
+    msg.info.y = position.y;
+    //msg.info.z = position.z;
+    msg.info.z = 0;
+    msg.info.position_error = pos_error.xy;
+    send_message(MSGID_POSITION, sizeof(msg.buf), msg.buf);
+}
+
+void send_message(uint8_t msg_id, uint8_t data_len, uint8_t data_buf[])
+{
+    // sanity check
+    if (data_len == 0) {
+        return;
+    }
+
+    // message is buffer length + 1 (for checksum)
+    uint8_t msg_len = data_len+1;
+
+    // calculate checksum and place in last element of array
+    uint8_t checksum = 0;
+    checksum ^= msg_id;
+    checksum ^= msg_len;
+    for (uint8_t i=0; i<data_len; i++) {
+        checksum = checksum ^ data_buf[i];
+    }
+
+    // send message
+    int16_t num_sent = 0;
+    num_sent += fcboardSerial.write(MSG_HEADER);
+    num_sent += fcboardSerial.write(msg_id);
+    num_sent += fcboardSerial.write(msg_len);
+    num_sent += fcboardSerial.write(data_buf, data_len);
+    num_sent += fcboardSerial.write(&checksum, 1);
+    fcboardSerial.flush();
+}
+