Dzollo/v2.3 heading fwd

Config.in

@@ -60,3 +60,4 @@ source "$BR2_EXTERNAL_piksi_buildroot_PATH/package/metrics_daemon/Config.in"
 source "$BR2_EXTERNAL_piksi_buildroot_PATH/package/resource_monitor/Config.in"
 source "$BR2_EXTERNAL_piksi_buildroot_PATH/package/cmph/Config.in"
 source "$BR2_EXTERNAL_piksi_buildroot_PATH/package/bcm_wrapper/Config.in"
+source "$BR2_EXTERNAL_piksi_buildroot_PATH/package/heading_forwarder/Config.in"

configs/fragments/piksiv3/core_packages

@@ -29,3 +29,4 @@ BR2_PACKAGE_OTA_DAEMON=y
 BR2_PACKAGE_METRICS_DAEMON=y
 BR2_PACKAGE_RESOURCE_MONITOR=y
 BR2_PACKAGE_PFW_WELCOME=y
+BR2_PACKAGE_HEADING_FORWARDER=y

configs/hashes/piksiv3_internal_defconfig

@@ -1 +1 @@
-7683ac789e8af433adce4a9a2e9225644f54a6ab2d2d6f0fc70c1af6456339b6
+67d894502fac9964e265299ed2b8e9dfd31daae6decd251805464dc99d9c7b72

configs/hashes/piksiv3_release_defconfig

@@ -1 +1 @@
-f953954273d6778472adfc91d84bf97ea1c9223c667e35e91f90fe6a94874412
+2516cb54791125d78a1f335ce03572e51114edf7c7c0fb098df35495c17ee655

configs/hashes/piksiv3_sdk_defconfig

@@ -1 +1 @@
-d51d13febb96495f6fe2512b5a73a0b427b5fc488d237fb9b9189bb87796d543
+c903f5b0f32a41a79582f81621f9748d291db48e3770ce18ae3a56a2b3fb95eb

configs/hashes/piksiv3_unprotected_defconfig

@@ -1 +1 @@
-1a91fe9a04ccd43fb417ca36554dcd04e2846165fbe70d540e0e5eb9d6d40285
+ef4eb89a2a673e800ffe57b81e97c798f9f27b3dff3789e9c4e224c63065aab8

configs/piksiv3_internal_defconfig

@@ -117,6 +117,7 @@ BR2_PACKAGE_OTA_DAEMON=y
 BR2_PACKAGE_METRICS_DAEMON=y
 BR2_PACKAGE_RESOURCE_MONITOR=y
 BR2_PACKAGE_PFW_WELCOME=y
+BR2_PACKAGE_HEADING_FORWARDER=y
 BR2_PACKAGE_PIKSI_INS=y
 BR2_PACKAGE_BUILD_TOOLS=y
 BR2_PACKAGE_PIKSI_INS_REF=y

configs/piksiv3_release_defconfig

@@ -117,6 +117,7 @@ BR2_PACKAGE_OTA_DAEMON=y
 BR2_PACKAGE_METRICS_DAEMON=y
 BR2_PACKAGE_RESOURCE_MONITOR=y
 BR2_PACKAGE_PFW_WELCOME=y
+BR2_PACKAGE_HEADING_FORWARDER=y
 BR2_PACKAGE_PIKSI_INS=y
 BR2_PACKAGE_BUILD_TOOLS=y
 BR2_PACKAGE_RELEASE_LOCKDOWN=y

configs/piksiv3_sdk_defconfig

@@ -117,5 +117,6 @@ BR2_PACKAGE_OTA_DAEMON=y
 BR2_PACKAGE_METRICS_DAEMON=y
 BR2_PACKAGE_RESOURCE_MONITOR=y
 BR2_PACKAGE_PFW_WELCOME=y
+BR2_PACKAGE_HEADING_FORWARDER=y
 BR2_PACKAGE_SAMPLE_DAEMON=y
 BR2_PACKAGE_PIKSI_DEV_TOOLS=y

configs/piksiv3_unprotected_defconfig

@@ -117,4 +117,5 @@ BR2_PACKAGE_OTA_DAEMON=y
 BR2_PACKAGE_METRICS_DAEMON=y
 BR2_PACKAGE_RESOURCE_MONITOR=y
 BR2_PACKAGE_PFW_WELCOME=y
+BR2_PACKAGE_HEADING_FORWARDER=y
 BR2_PACKAGE_RELEASE_LOCKDOWN=y

package/heading_forwarder/Config.in

@@ -0,0 +1,5 @@
+config BR2_PACKAGE_HEADING_FORWARDER
+  bool "heading_forwarder"
+  select BR2_PACKAGE_LIBUV
+  select BR2_PACKAGE_LIBSBP
+  select BR2_PACKAGE_LIBPIKSI

package/heading_forwarder/heading_forwarder.mk

@@ -0,0 +1,25 @@
+################################################################################
+#
+# heading_forwarder
+#
+################################################################################
+
+HEADING_FORWARDER_VERSION = 0.1
+HEADING_FORWARDER_SITE = \
+  "${BR2_EXTERNAL_piksi_buildroot_PATH}/package/heading_forwarder/src"
+HEADING_FORWARDER_SITE_METHOD = local
+HEADING_FORWARDER_DEPENDENCIES = libuv libsbp libpiksi
+
+define HEADING_FORWARDER_USERS
+	hdgfwd -1 hdgfwd -1 * - - -
+endef
+
+define HEADING_FORWARDER_BUILD_CMDS
+	$(MAKE) CC=$(TARGET_CC) LD=$(TARGET_LD) -C $(@D) all
+endef
+
+define HEADING_FORWARDER_INSTALL_TARGET_CMDS
+	$(INSTALL) -D -m 0755 $(@D)/heading_forwarder $(TARGET_DIR)/usr/bin
+endef
+
+$(eval $(generic-package))

package/heading_forwarder/src/Makefile

@@ -0,0 +1,16 @@
+TARGET=heading_forwarder
+SOURCES=\
+	heading_forwarder.c
+LIBS=-luv -lsbp -lpiksi -lsettings
+CFLAGS=-std=gnu11 -Wmissing-prototypes -Wconversion -Wimplicit -Wshadow -Wswitch-default -Wswitch-enum -Wundef -Wuninitialized -Wpointer-arith -Wstrict-prototypes -Wcast-align -Wformat=2 -Wimplicit-function-declaration -Wredundant-decls -Wformat-security -Wall -Wextra -Wno-strict-prototypes -Werror
+
+CROSS=
+
+CC=$(CROSS)gcc
+
+all:	$(TARGET)
+$(TARGET): $(SOURCES)
+	$(CC) $(CFLAGS) -o $(TARGET) $(SOURCES) $(LIBS)
+
+clean:
+	rm -rf $(TARGET)

package/heading_forwarder/src/heading_forwarder.c

@@ -0,0 +1,148 @@
+/*
+ * Copyright (C) 2019 Swift Navigation Inc.
+ * Contact: Swift Navigation <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#include <getopt.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <libpiksi/logging.h>
+#include <libpiksi/sbp_pubsub.h>
+#include <libpiksi/loop.h>
+
+#include <libsbp/orientation.h>
+#include <libsbp/navigation.h>
+
+#define PROGRAM_NAME "heading_forwarder"
+
+#define SBP_SUB_ENDPOINT "ipc:///var/run/sockets/heading_forward.pub" /* SBP Heading Out */
+#define SBP_PUB_ENDPOINT "ipc:///var/run/sockets/heading_forward.sub" /* SBP Heading In */
+
+static void usage(char *command)
+{
+  printf("Usage: %s\n", command);
+
+  puts("\nMain options");
+}
+
+static int parse_options(int argc, char *argv[])
+{
+  // clang-format off
+  const struct option long_opts[] = {
+    { 0, 0, 0, 0 },
+  };
+  // clang-format on
+
+  int opt;
+  while ((opt = getopt_long(argc, argv, "", long_opts, NULL)) != -1) {
+    switch (opt) {
+
+    default: {
+      puts("Invalid option");
+      return -1;
+    } break;
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief baseline_heading_callback - forward baseline heading messages with
+ */
+static void baseline_heading_callback(u16 sender_id, u8 len, u8 msg_[], void *context)
+{
+  sbp_pubsub_ctx_t *pubsub_ctx = (sbp_pubsub_ctx_t *)context;
+
+  if (sbp_tx_send_from(sbp_pubsub_tx_ctx_get(pubsub_ctx),
+                       SBP_MSG_BASELINE_HEADING,
+                       len,
+                       msg_,
+                       sender_id)
+      < 0) {
+    PK_LOG_ANNO(LOG_ERR, "Failed to forward baseline heading from sender_id: %d", sender_id);
+  }
+}
+
+/**
+ * @brief baseline_ned_callback - forward baseline ned messages with
+ */
+static void baseline_ned_callback(u16 sender_id, u8 len, u8 msg_[], void *context)
+{
+  sbp_pubsub_ctx_t *pubsub_ctx = (sbp_pubsub_ctx_t *)context;
+
+  if (sbp_tx_send_from(sbp_pubsub_tx_ctx_get(pubsub_ctx),
+                       SBP_MSG_BASELINE_NED,
+                       len,
+                       msg_,
+                       sender_id)
+      < 0) {
+    PK_LOG_ANNO(LOG_ERR, "Failed to forward baseline ned from sender_id: %d", sender_id);
+  }
+}
+
+
+static int cleanup(pk_loop_t **pk_loop_loc, sbp_pubsub_ctx_t **pubsub_ctx_loc, int status);
+
+int main(int argc, char *argv[])
+{
+  pk_loop_t *loop = NULL;
+  sbp_pubsub_ctx_t *pubsub_ctx = NULL;
+
+  logging_init(PROGRAM_NAME);
+
+  if (parse_options(argc, argv) != 0) {
+    piksi_log(LOG_ERR, "invalid arguments");
+    usage(argv[0]);
+    exit(cleanup(&loop, &pubsub_ctx, EXIT_FAILURE));
+  }
+
+  loop = pk_loop_create();
+  if (loop == NULL) {
+    exit(cleanup(&loop, &pubsub_ctx, EXIT_FAILURE));
+  }
+
+  pubsub_ctx = sbp_pubsub_create(PROGRAM_NAME, SBP_PUB_ENDPOINT, SBP_SUB_ENDPOINT);
+  if (pubsub_ctx == NULL) {
+    exit(cleanup(&loop, &pubsub_ctx, EXIT_FAILURE));
+  }
+
+  if (sbp_rx_attach(sbp_pubsub_rx_ctx_get(pubsub_ctx), loop) != 0) {
+    exit(cleanup(&loop, &pubsub_ctx, EXIT_FAILURE));
+  }
+
+  sbp_rx_callback_register(sbp_pubsub_rx_ctx_get(pubsub_ctx),
+                           SBP_MSG_BASELINE_HEADING,
+                           baseline_heading_callback,
+                           pubsub_ctx,
+                           NULL);
+
+  sbp_rx_callback_register(sbp_pubsub_rx_ctx_get(pubsub_ctx),
+                           SBP_MSG_BASELINE_NED,
+                           baseline_ned_callback,
+                           pubsub_ctx,
+                           NULL);
+
+  pk_loop_run_simple(loop);
+
+  exit(cleanup(&loop, &pubsub_ctx, EXIT_SUCCESS));
+}
+
+static int cleanup(pk_loop_t **pk_loop_loc, sbp_pubsub_ctx_t **pubsub_ctx_loc, int status)
+{
+  pk_loop_destroy(pk_loop_loc);
+  if (*pubsub_ctx_loc != NULL) {
+    sbp_pubsub_destroy(pubsub_ctx_loc);
+  }
+  logging_deinit();
+
+  return status;
+}

package/piksi_system_daemon/src/main.c

@@ -49,6 +49,7 @@ static double network_polling_frequency = 0.1;
 static double network_polling_retry_frequency = 1;
 static char network_polling_addresses[1024] = "8.8.8.8";
 static bool log_ping_activity = false;
+static bool heading_forward = false;
 
 #define RUNIT_SERVICE_DIR "/var/run/piksi_system_daemon/sv"
 /*#define DEBUG_PIKSI_SYSTEM_DAEMON*/
@@ -589,6 +590,49 @@ static int network_polling_notify(void *context)
   return SETTINGS_WR_OK;
 }
 
+static int heading_forward_notify(void *context)
+{
+  (void)context;
+
+  /* clang-format off */
+  static runit_config_t cfg = (runit_config_t) {
+    .service_dir    = RUNIT_SERVICE_DIR,
+    .service_name   = "heading_forwarder",
+    .command_line   = "heading_forwarder",
+    .custom_down    = NULL,
+    .finish_command = NULL,
+    .restart        = true,
+  };
+  /* clang-format on */
+
+  runit_stat_t stat = stat_runit_service(&cfg);
+
+  switch (stat) {
+  case RUNIT_UNKNOWN:
+  case RUNIT_NO_STAT:
+  case RUNIT_NO_PID:
+  case RUNIT_DOWN: {
+    if (heading_forward == true) {
+      if (start_runit_service(&cfg) != 0) {
+        return SETTINGS_WR_SERVICE_FAILED;
+      }
+    }
+  } break;
+
+  case RUNIT_RUNNING:
+  case RUNIT_RUNNING_OTHER:
+  default: {
+    if (heading_forward == false) {
+      if (stop_runit_service(&cfg) != 0) {
+        return SETTINGS_WR_SERVICE_FAILED;
+      }
+    }
+  }
+  }
+
+  return SETTINGS_WR_OK;
+}
+
 int main(void)
 {
   logging_init(PROGRAM_NAME);
@@ -712,6 +756,14 @@ int main(void)
                        SETTINGS_TYPE_BOOL,
                        network_polling_notify,
                        NULL);
+  pk_settings_register(settings_ctx,
+                       "system",
+                       "heading_forwarding",
+                       &heading_forward,
+                       sizeof(heading_forward),
+                       SETTINGS_TYPE_BOOL,
+                       heading_forward_notify,
+                       NULL);
 
   sbp_rx_callback_register(sbp_pubsub_rx_ctx_get(pubsub_ctx),
                            SBP_MSG_RESET,

package/sbp_protocol/src/sbp_router.yml

@@ -129,6 +129,11 @@ ports:
         filters:
           - { action: ACCEPT, prefix: [0x55, 0x4A, 0x00] } # SBP_MSG_OBS
           - { action: REJECT }
+      - dst_port: SBP_PORT_HEADING_FORWARD
+        filters:
+          - { action: ACCEPT, prefix: [0x55, 0x0F, 0x02] } # SBP_MSG_BASELINE_HEADING
+          - { action: ACCEPT, prefix: [0x55, 0x0C, 0x02] } # SBP_MSG_BASELINE_NED
+          - { action: REJECT }
 
   - name: SBP_PORT_FILEIO_FIRMWARE
     metric: "sbp/fileio_fw"
@@ -244,3 +249,12 @@ ports:
           - { action: ACCEPT, prefix: [0x55, 0x06, 0x7F] } # MSG_LINUX_PROCESS_FD_COUNT
           - { action: ACCEPT, prefix: [0x55, 0x07, 0x7F] } # MSG_LINUX_PROCESS_FD_SUMMARY
           - { action: REJECT }
+
+  - name: SBP_PORT_HEADING_FORWARD
+    metric: "sbp/heading_forward"
+    pub_addr: "ipc:///var/run/sockets/heading_forward.pub"
+    sub_addr: "ipc:///var/run/sockets/heading_forward.sub"
+    forwarding_rules:
+      - dst_port: SBP_PORT_EXTERNAL
+        filters:
+          - { action: ACCEPT }