Merge submodule contents for sololink/master

sololink/.clang-format

@@ -0,0 +1,19 @@
+---
+BasedOnStyle: LLVM
+AccessModifierOffset: -4
+AllowShortFunctionsOnASingleLine: false
+BreakBeforeBinaryOperators: true
+AlwaysBreakTemplateDeclarations: true
+ColumnLimit: 100
+Standard: Cpp11
+IndentWidth: 4
+UseTab: Never
+BreakBeforeBraces: Linux
+#AlignConsecutiveAssignments: true # only available as of clang-format 3.7
+BreakBeforeBinaryOperators: false
+PointerAlignment: Right
+# SpacesInAngles needed for nested templates (gcc 4.8.4 errors)
+SpacesInAngles: true
+---
+# other lang configs here...
+...

sololink/.gitignore

@@ -0,0 +1,14 @@
+# osx junk
+*.DS_Store
+
+# object files
+*.o
+*.pyc
+
+# pycharm files
+.idea
+
+# logs from sim
+*.tlog
+*.tlog.raw
+*.parm

sololink/COPYRIGHT-3DR

@@ -0,0 +1,37 @@
+Copyright 2014-2017 3D Robotics
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use these files except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+~~~~
+
+Contributors prior to public release:
+
+  * Eric Liao
+  * John Finley
+  * Allan Matthew
+  * Will Silva
+  * Liam Staskawicz
+  * Angus Peart
+  * Igor Napolskikh
+  * Tim Ryan
+  * Peter Barker
+  * Robert Cottrell
+  * Ramón Roche
+  * Adam Setapen
+  * Kevin Mehall
+  * Jonathan Challinger
+  * Nick Speal
+  * Siddharth Bharat Purohit
+  * Andrew Tridgell
+  * Daniel Nugent
+  * Chavi Weingarten

sololink/LICENSE-APACHE

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

sololink/Makefile

@@ -0,0 +1,30 @@
+
+SUBDIRS = flightcode wifi
+SUBDIRS_BUILD = $(SUBDIRS:%=%_build)
+SUBDIRS_CLEAN = $(SUBDIRS:%=%_clean)
+
+# SUBDIRS2 should eventually be same as SUBDIRS when all are formatted
+
+SUBDIRS2 = flightcode
+SUBDIRS_FMT = $(SUBDIRS2:%=%_fmt)
+SUBDIRS_FMT_DIFF = $(SUBDIRS2:%=%_fmt-diff)
+
+all: $(SUBDIRS_BUILD)
+
+build: $(SUBDIRS_BUILD)
+$(SUBDIRS_BUILD):
+	$(MAKE) -C $(@:%_build=%)
+
+clean: $(SUBDIRS_CLEAN)
+$(SUBDIRS_CLEAN):
+	$(MAKE) -C $(@:%_clean=%) clean
+
+fmt: $(SUBDIRS_FMT)
+$(SUBDIRS_FMT):
+	$(MAKE) -C $(@:%_fmt=%) fmt
+
+fmt-diff: $(SUBDIRS_FMT_DIFF)
+$(SUBDIRS_FMT_DIFF):
+	$(MAKE) -C $(@:%_fmt-diff=%) fmt-diff
+
+.PHONY: $(SUBDIRS) $(SUBDIRS_BUILD) $(SUBDIRS_CLEAN) $(SUBDIRS_FMT) $(SUBDIRS_FMT_DIFF)

sololink/README.md

@@ -0,0 +1,13 @@
+WARNING - WORK IN PROGRESS
+
+```
+This code is known to be high risk , but also early-adopter friendly.  
+
+We will remove this warning from the repository when it is no longer required.
+```
+
+
+SoloLink
+========
+
+Software for the SoloLink

sololink/SOURCE

@@ -0,0 +1 @@
+This source code was released by 3DR in the file: sololink_2.2.4_ff71bb8f.tar.gz

sololink/STM32Loader/checkArtooAndUpdate.py

@@ -0,0 +1,292 @@
+#!/usr/bin/env python
+
+import glob
+import serial
+import slip
+import os
+import subprocess
+import sys
+import time
+import logging
+import logging.config
+import param_stored_vals_msg  # Used for reading stick calibration values
+import slip, struct  # Used for writing stick calibration values
+
+sololink_conf = "/etc/sololink.conf"
+
+ARTOO_SYSINFO_ID = chr(0x3)
+ARTOO_UPDATE_ID = chr(0x12)
+ARTOO_LOCKOUT_ID = chr(0x13)
+ARTOO_CALIBRATE_ID = chr(0x2)
+
+ARTOO_UPDATE_SUCCESS = chr(1)
+ARTOO_UPDATE_FAILED = chr(2)
+
+ARTOO_LOCKOUT_FALSE = chr(0)
+ARTOO_LOCKOUT_TRUE = chr(1)
+
+ARTOO_BAUD = 115200
+
+# update_result should be either ARTOO_UPDATE_SUCCESS or ARTOO_UPDATE_FAILED
+def setArtooUpdateComplete(update_result):
+    ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1)
+    slipdev = slip.SlipDevice(ser)
+    slipdev.write("".join([ARTOO_UPDATE_ID, update_result]))
+    ser.close()
+
+# lockout should be either ARTOO_LOCKOUT_FALSE or ARTOO_LOCKOUT_TRUE
+def setArtooLockout(lockout):
+    ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1)
+    slipdev = slip.SlipDevice(ser)
+    slipdev.write("".join([ARTOO_LOCKOUT_ID, lockout]))
+    ser.close()
+
+def mkdir_p(path):
+    try:
+        os.makedirs(path)
+    except OSError:
+        pass # already exists
+
+def doUpdateComplete():
+    setArtooLockout(ARTOO_LOCKOUT_FALSE)
+    if not os.path.exists("/log/updates/READY"):
+        if os.path.exists("/log/updates/UPDATEFAILED"):
+            f = open("/log/updates/UPDATEFAILED", "r")
+            # file should be one line; read it all
+            r = f.read(1000)
+            f.close()
+            r = r.strip("\r\n\t\0 ")
+            logger.info("request \"update failed\" screen (%s)", r)
+            setArtooUpdateComplete(ARTOO_UPDATE_FAILED)
+        else:
+            logger.info("request \"update success\" screen")
+            setArtooUpdateComplete(ARTOO_UPDATE_SUCCESS)
+        mkdir_p("/log/updates")
+        open("/log/updates/READY", "w").close() # "touch"
+    else:
+        logger.info("no screen update (READY exists)")
+
+# return tuple (filename, version), or None
+def getFirmwareInfo():
+    files = sorted(glob.glob("/firmware/artoo_*.bin"))
+    if not files:
+        return None
+    filename = files[-1]
+    # Filename may be of the form
+    # "/firmware/artoo_0.0.0.bin", or
+    # "/firmware/artoo_v0.0.0.bin".
+    # Get it without the 'v'.
+    if filename[16] == 'v':
+        version = filename[17:-4]
+    else:
+        version = filename[16:-4]
+    return (filename, version)
+
+# return version as string ("unknown" if can't get version)
+def getArtooVersion():
+    #Check the version of the stm32 firmware over serial
+    #The STM32 might be emitting packets already, so we try a few times to get the
+    #version string. This has been observed to get the version with one retry on
+    #several occasions.
+    logger.info("requesting stm32 version")
+    version = "unknown"
+    ser = serial.Serial("/dev/ttymxc1", 115200, timeout=1)
+    slipdev = slip.SlipDevice(ser)
+    for i in range(5):
+        slipdev.write("".join([ARTOO_SYSINFO_ID]))
+        pkt = slipdev.read()
+        if not pkt:
+            logger.info("no data received from stm32, retrying")
+            continue
+        pkt = "".join(pkt)
+        if pkt[0] == ARTOO_SYSINFO_ID:
+            # SysInfo packet is:          artoo/src/hostprotocol.cpp
+            # start size
+            #   0     1  ARTOO_SYSINFO_ID artoo/src/hostprotocol.h
+            #   1    12  UniqueId         artoo/src/stm32/sys.h
+            #  13     2  hwversion        artoo/src/hostprotocol.cpp
+            #  15   var  Version          artoo/src/version.h
+            # Version may start with an initial 'v', e.g. v0.6.10,
+            # but we want it starting with the numeric part.
+            if pkt[15] != 'v':
+                version = pkt[15:]
+            else:
+                version = pkt[16:]
+            break
+        logger.info("got %s/%d, retrying", str(hex(ord(pkt[0]))), len(pkt))
+    ser.close()
+    return version
+
+# Attempt to update the STM32 multiple times
+def updateStm32(filename):
+    # Total attempts will be retry+1, with the last one doing the unprotect.
+    # Since doing the unprotect results in a support call, retries is set
+    # such that the unprotect is a last resort. Allow for two init failures
+    # and two erase failures (truly degenerate) before doing the unprotect
+    # and (hopefully) fixing a protected stm32. Getting to the unprotect
+    # (where it is really needed) should happen only once in a controller's
+    # lifetime, if at all (it should never be needed, really).
+    stick_cal = None
+    retry = 4
+    while retry >= 0:
+        if retry > 0:
+            # don't unprotect chip the first few times an erase fails
+            logger.info("updating without readout protection disable")
+            if call_stm32loader(filename, disable_readout_protect=False):
+                break  # Move on if the firmware is successfully loaded.
+        else:
+            # last attempt - unprotect chip if erase fails
+            logger.info("Updating the STM32 has failed several times. We suspect that readout protection is enabled.")
+            logger.info("Saving Stick Calibration: ")
+            stick_cal = read_stick_cal()
+            logger.info(stick_cal)
+            logger.info("updating with readout protection disable. This will wipe flash.")
+            call_stm32loader(filename, disable_readout_protect=True)
+        retry -= 1
+
+    logger.info("Update complete. Stick cal before update was: {}. (None means it wasn't read before update)".format(stick_cal))
+    logger.info("stick cal after update: {}".format(read_stick_cal()))
+
+    # Only write stick calibration if we did disabled readout protection:
+    if stick_cal is not None:
+        # Try to write_stick cal multiple times in case it fails.
+        # This is defensive. I've never seen it fail.
+        for i in range(3):
+            write_stick_cal(stick_cal)
+            time.sleep(1)
+            logger.info("Stick cal written. Reading it back again as a check:")
+            new_cal = read_stick_cal()
+            logger.info(new_cal)
+            if stick_cal == new_cal:
+                break
+            else:
+                logger.info("Stick cal failed to be written correctly:")
+                logger.info("Calibration attempted to be written: {}".format(stick_cal))
+                logger.info("Calibration found in flash: {}".format(new_cal))
+
+# Initiate the stm32loader process to actually update the STM32 chip. Return True if successful.
+def call_stm32loader(filename, disable_readout_protect=False):
+    # subprocess.check_output raises an exception if stm32loader.py returns a nonzero exit code for any reason
+    # including if readout_protect is enabled.
+    # Disable readout protect if specified. This will erase all flash.
+
+    if disable_readout_protect:
+        flags = "-wvqu"
+    else:
+        flags = "-wvq"
+
+    try:
+        s = subprocess.check_output(["stm32loader.py", flags, "-s", "127",
+                                     "-b", "115200", "-p", "/dev/ttymxc1", filename],
+                                    stderr=subprocess.STDOUT)
+        logger.info("stm32loader.py returned normally; output:")
+        success = True
+    except subprocess.CalledProcessError as cpe:
+        s = cpe.output
+        logger.info("stm32loader.py returned error; output:")
+        success = False
+
+    # this might be ugly, but it gets the output in the log
+    s = s.strip("\r\n\t\0 ")
+    logger.info(s)
+    # Wait a second for the STM32 to come back up before we send it a message later
+    time.sleep(1)
+    return success
+
+def writeVersionFile(version):
+    f = open("/STM_VERSION", 'w')
+    f.write(version + '\n')
+    f.close()
+
+# return version from /STM_VERSION
+def getVersionFile():
+    try:
+        f = open("/STM_VERSION", 'r')
+        version = f.readline()
+        f.close()
+        version = version.strip()
+        if version == "":
+            version = "unknown"
+    except:
+        version = "unknown"
+    return version
+
+
+def read_stick_cal():
+    # Start the STM32 process because sololink_config sets runlevel to 2 before an update,
+    # but we need the (runlevel 3) STM32 process to read a stick cal.
+    p = subprocess.Popen('stm32', shell=True)
+    time.sleep(3)
+
+    msg = param_stored_vals_msg.fetch()  # String containing all artoo params (including stick cal values)
+    p.terminate()  # Kill the STM32 process.
+    time.sleep(1)  # Give time for the STM32 process to die. Not sure if needed.
+
+    # msg is an empty string if the stm32 process is not running (runlevel 2)
+    if not msg:
+        logger.info("Could not read stick calibration.")
+        return None
+    else:
+        msg = param_stored_vals_msg.unpack(msg)  # Converts string to Dict
+        axes = msg.get('stickCals')  # 6-item list of 3-item tuples
+        # Pack datastructure into a single list that can be written later
+        stick_cals = []
+        for a in axes:
+            stick_cals.extend(a)
+        return stick_cals
+
+
+def write_stick_cal(values):
+    logger.info("Writing stick cal: {}".format(values))
+
+    serialpath = '/dev/ttymxc1'
+    serialport = serial.Serial(serialpath, ARTOO_BAUD)
+    slipdev = slip.SlipDevice(serialport)
+
+    packed_vals = struct.pack("<HHHHHHHHHHHHHHHHHH", *values)
+    slipdev.write("".join([ARTOO_CALIBRATE_ID, packed_vals]))
+
+
+# Main
+logging.config.fileConfig(sololink_conf)
+logger = logging.getLogger("stm32")
+
+logger.info("stm32 update starting")
+
+firmware = getFirmwareInfo()
+
+if firmware is None:
+    logger.info("no firmware available for update")
+else:
+    logger.info("firmware: file %s, version %s", firmware[0], firmware[1])
+
+# Read the version from the STM32
+artoo_version = getArtooVersion()
+logger.info("running version: %s", artoo_version)
+
+# If we have firmware and it does not match what is running, update the STM32
+if firmware is not None:
+    if artoo_version != firmware[1]:
+        logger.info("updating")
+        updateStm32(firmware[0])
+        # re-read the version from the running firmware
+        artoo_version = getArtooVersion()
+    else:
+        logger.info("not updating (new firmware is already running)")
+    # Whether we used it or not, we are done with the new firmware
+    logger.info("moving firmware to loaded")
+    mkdir_p("/firmware/loaded")
+    os.rename(firmware[0], "/firmware/loaded/" + os.path.basename(firmware[0]))
+else:
+    logger.info("not updating (no new firmware)")
+
+# Write version retrieved from STM32 to file
+logger.info("writing STM_VERSION with running version %s", artoo_version)
+writeVersionFile(artoo_version)
+
+doUpdateComplete()
+
+# delete /log/.factory if it exists (it has no effect)
+if os.path.exists("/log/.factory"):
+    logger.info("deleting .factory")
+    os.system("rm -f /log/.factory")

sololink/STM32Loader/checkArtooAndUpdateTest

@@ -0,0 +1,96 @@
+#!/bin/bash
+
+# Conditions:
+# Firmware file: does not exist, exists and is same, exists and is different
+# STM_VERSION: does not exist, exists
+
+# start out with nothing in /firmware,
+# and two versions in /firmware/loaded
+version_1="0.6.14"
+version_2="0.7.3"
+
+find /firmware -name \*.bin
+
+init 2
+sleep 5
+
+# start with version_1 running
+date +%H:%M:%S
+stm32loader.py -wvq -s 127 -b 115200 -p /dev/ttymxc1 /firmware/loaded/artoo_${version_1}.bin
+date +%H:%M:%S
+
+# test: (firmware file, STM_VERSION)
+
+# ***** should not happen
+echo
+echo "TEST: no update, /STM_VERSION does not exist"
+rm /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_1}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+
+# test: (does not exist, exists)
+# ***** common boot case
+echo
+echo "TEST: no update, /STM_VERSION exists"
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_1}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+
+# ***** possible first-boot case
+echo
+echo "TEST: update exists but is same, /STM_VERSION does not exist"
+mv /firmware/loaded/artoo_${version_1}.bin /firmware
+rm /STM_VERSION
+echo "${version_1} in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_1}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+
+# ***** uncommon update case
+echo
+echo "TEST: update exists but is same, /STM_VERSION exists"
+mv /firmware/loaded/artoo_${version_1}.bin /firmware
+echo "${version_1} in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_1}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+
+# ***** possible first-boot case
+echo
+echo "TEST: update exists and is different, /STM_VERSION does not exist"
+mv /firmware/loaded/artoo_${version_2}.bin /firmware
+rm /STM_VERSION
+echo "${version_2} in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_2}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin
+
+# ***** common update case
+echo
+echo "TEST: update exists and is different, /STM_VERSION exists"
+mv /firmware/loaded/artoo_${version_1}.bin /firmware
+echo "${version_1} in /firmware:"
+find /firmware -name \*.bin
+checkArtooAndUpdate.py
+echo "/STM_VERSION should be ${version_1}:"
+cat /STM_VERSION
+echo "nothing in /firmware:"
+find /firmware -name \*.bin

sololink/STM32Loader/reset_artoo

@@ -0,0 +1,63 @@
+#!/bin/sh
+
+# -d 'N': delay 'N' seconds during reset
+# -n: don't kill stm32 process
+
+usage() {
+  echo "usage: reset_artoo [-d N] [-n]"
+  echo "  -d N    delay N seconds with reset asserted"
+  echo "  -n      don't kill the stm32 process (debug usage)"
+}
+
+delay=0
+dokill=true
+until [ -z "${1}" ]; do
+  case ${1} in
+    -d)
+      if [ -z "${2}" ]; then
+        usage
+      fi
+      delay="${2}"
+      shift
+      ;;
+    -n)
+      dokill=false
+      ;;
+  esac
+  shift
+done
+
+pin_boot=45
+pin_reset=46
+
+set_pin() {
+  pushd /sys/class/gpio > /dev/null
+  if [ ! -e gpio${1} ]; then
+    echo "${1}" > /sys/class/gpio/export
+  fi
+  echo "out" > /sys/class/gpio/gpio${1}/direction
+  echo "${2}" > /sys/class/gpio/gpio${1}/value
+  popd > /dev/null
+}
+
+if ${dokill}; then
+  # stop it
+  init 2
+  # wait for it to be truly gone
+  until ! killall -q -CONT stm32; do
+    true
+  done
+fi
+
+# deassert boot
+set_pin ${pin_boot} 0
+
+# pulse reset
+set_pin ${pin_reset} 1
+sleep ${delay}
+set_pin ${pin_reset} 0
+
+if ${dokill}; then
+  # restart everything
+  init 3
+fi

sololink/STM32Loader/stm32_check_cal.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+
+# stm32loader in /usr/bin is not on the default python path; make
+# sure we always find it no matter where this script is installed
+import sys
+sys.path.append("/usr/bin")
+import stm32loader
+
+stm32loader.QUIET = 0
+
+cmd = stm32loader.CommandInterface()
+cmd.open("/dev/ttymxc1", 115200)
+cmd.initChip()
+cmd.cmdGet()
+cmd.cmdGetID()
+
+status = 1
+
+msg = "cal is blank"
+
+try:
+    data = cmd.readMemory(0x0803f800, 2048)
+    for c in data:
+        if c != 255:
+            msg = "cal is not blank"
+            status = 0
+            break
+except:
+    msg = "error reading cal (readout protect?)"
+    pass
+
+cmd.releaseChip()
+
+print msg
+
+sys.exit(status)

sololink/STM32Loader/stm32_readout_protect.py

@@ -0,0 +1,15 @@
+#!/usr/bin/env python
+
+# stm32loader in /usr/bin is not on the default python path; make
+# sure we always find it no matter where this script is installed
+import sys
+sys.path.append("/usr/bin")
+import stm32loader
+
+cmd = stm32loader.CommandInterface()
+cmd.open("/dev/ttymxc1", 115200)
+cmd.initChip()
+cmd.cmdGet()
+cmd.cmdGetID()
+cmd.cmdReadoutProtect()
+cmd.releaseChip()

sololink/STM32Loader/stm32_reset.py

@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+
+# stm32loader in /usr/bin is not on the default python path; make
+# sure we always find it no matter where this script is installed
+import sys
+sys.path.append("/usr/bin")
+import stm32loader
+
+cmd = stm32loader.CommandInterface()
+cmd.open("/dev/ttymxc1", 115200)
+cmd.initChip()
+cmd.cmdGet()
+cmd.cmdGetID()
+cmd.releaseChip()

sololink/STM32Loader/stm32_update_test.sh

@@ -0,0 +1,149 @@
+#!/bin/bash
+
+# STM32 Update Testing
+#
+# forever:
+#   "update" to factory
+#   verify calibration
+#   update to new
+#   verify calibration
+#
+# Setup:
+#   Put factory and latest firmware on controller in /firmware/loaded.
+#   Copy this script to controller and run it there.
+#   Set names of those two files in this script (below).
+#
+# Results go to stdout. Run it like this:
+#   # ./stm32_update_test.sh 2>&1 | tee -a /log/stm32_test.log &
+# to get a log (of both stdout and stderr) and let you disconnect from
+# controller while the test runs. If cal is ever detected as missing,
+# the test stops.
+#
+# BIG NOTE:
+#
+# If you run the script in the background (&), then it is "difficult"
+# (I could not figure out how) to stop it without leaving the STM32 in
+# a corrupt state (no program to run). If you turn off controller in
+# this state, you will have to OPEN IT UP AND FLASH THE STM32 MANUALLY
+# to fix it. "Safely" (relative, right) terminate the test like this:
+#   # ./stm32_update_test.sh 2>&1 | tee -a /log/stm32_test.log &
+#   [1] 5970
+#   #
+#   (time passes)
+#   # kill %1
+#   (STM32 is mostly likely CORRUPT at this point)
+#   # mv /firmware/loaded/artoo_1.2.8.bin /firmware
+#   # checkArtooAndUpdate.py
+#   (STM32 is flashed)
+#
+# Little Notes:
+# * Stm32 is normally receiving data (mavlink) at the moment the update
+#   process begins. One thought is that data might somehow remain in the
+#   uart (imx6 -> STM32), causing problems in init. This test probably
+#   does not cover that, but another test that *did* catch that shows
+#   that the effect is to fail to flash artoo - cal is not erased.
+# * After killing this test, you may be in runlevel 2, which means the
+#   controller's power button will NOT work; do an "init 3" then it will
+#   work. If you control-C, the script will finish the current update
+#   (up to a minute) and quit, leaving you in runlevel 3.
+
+factory=artoo_1.2.2.bin
+latest=artoo_1.2.8.bin
+
+# $1 is start address
+# $2 is length
+# $3 is output file
+stm32_read() {
+    init 2
+    sleep 2
+    stm32loader.py -q -p /dev/ttymxc1 -r -a $1 -l $2 $3
+    sleep 1
+    init 3
+    sleep 2
+}
+
+# $1 is the firmware filename
+stm32_update() {
+    cp /firmware/loaded/$1 /firmware
+    init 2
+    sleep 2
+    checkArtooAndUpdate.py
+    init 3
+    sleep 2
+}
+
+check_cal() {
+    stm32_read 0x0803f800 2048 /log/cal.bin
+    if cmp -s /log/cal.bin /log/cal-ref.bin; then
+        return 0
+    else
+        echo "cal read from STM32 does not match reference cal"
+        echo "reference cal:"
+        od -x /log/cal-ref.bin
+        echo "cal read from STM32:"
+        od -x /log/cal.bin
+        exit 1
+    fi
+}
+
+# check required files are there
+if [ ! -f /firmware/loaded/${factory} -o \
+     ! -f /firmware/loaded/${latest} ]; then
+    echo "${factory} or ${latest} not found in /firmware/loaded"
+    exit 1
+fi
+
+# get reference cal
+echo "reading reference cal..."
+stm32_read 0x0803f800 2048 /log/cal-ref.bin
+if [ `od -x /log/cal-ref.bin | wc -l` -le 3 ]; then
+    # cal looks erased already
+    echo "reference cal already erased?"
+    od -x /log/cal-ref.bin
+    exit 1
+fi
+
+# get erased block (assumes block before cal is erased)
+#echo "reading erased block..."
+#stm32_read 0x0803f000 2048 /log/cal-bad.bin
+#if [ `od -x /log/cal-bad.bin | wc -l` -ne 3 ]; then
+#    # does not look erased
+#    echo "erased block isn't erased?"
+#    od -x /log/cal-bad.bin
+#    exit 1
+#fi
+
+# set up so control-C (SIGINT) or kill (SIGTERM) exits between updates
+quitting=false
+
+on_sigint() {
+    echo "sigint..."
+    quitting=true
+}
+trap on_sigint SIGINT
+
+# The intent is to be able to background this script, then tell it to quit
+# with "kill %1" to send it a SIGTERM, but this never gets called... :(
+on_sigterm() {
+    echo "sigterm..."
+    quitting=true
+}
+trap on_sigterm SIGTERM
+
+while true; do
+
+    echo "updating to ${factory}..."
+    stm32_update ${factory}
+    echo "checking cal..."
+    check_cal
+
+    if [ $quitting == "true" ]; then break; fi
+
+    echo "updating to ${latest}..."
+    stm32_update ${latest}
+    echo "checking cal..."
+    check_cal
+
+    if [ $quitting == "true" ]; then break; fi
+
+done

sololink/STM32Loader/test_save_stick_cal.sh

@@ -0,0 +1,24 @@
+#!/bin/bash
+
+# Purpose of this test
+    # Enable readout protect and then attempt an artoo update.
+    # Artoo parameters (i.e. stick cals) should not be wiped.
+    # Test for IG-1435
+
+# Note that this test doesn't always work
+    # because for some reason I need to run stm32_readout_protect.py a couple times for it to take effect,
+    # but it should be kept around as inspiration for how to test this functionality manually.
+    # Testing via a hard power cycle could be more representative than reboot anyway
+
+# Preconditions for this test:
+    # Controller is on Solo v2.4.2-1, with Artoo 1.2.11.
+    # A firmware file for Artoo 1.2.11 named 1.2.22 exists in /firmware/loaded
+    # stm32_readout_protect.py and this file have been copied from the sololink repo (stm32loader folder) into artoo:/usr/bin
+
+init 2  # Stop the stm32 process
+sleep 5
+cp /firmware/loaded/artoo_1.2.12.bin /firmware/
+python /usr/bin/stm32_readout_protect.py  # Enable Readout Protect
+echo "Rebooting the controller. Once it comes up, check /log/3dr-solo.log for test results."
+sync
+reboot

sololink/STM32Loader/updateArtoo.sh

@@ -0,0 +1,28 @@
+#!/bin/sh
+
+#Comment out the RCTX line in /etc/inittab
+sed -i 's/^RCTX.*/#&/g' /etc/inittab
+
+#stop the stm32 process
+init q
+sleep 1
+
+# update artoo
+# we only erase the first 127 pages since the stm32 stores param data in its last page.
+# the stm32 we're currently using has 128 pages of 2k each - this will need to change if we
+# ever use different stm32 varieties.
+stm32loader.py -vw -s 127 -p /dev/ttymxc1 /home/root/artoo.bin
+
+# note - verification
+# you can read back the last page to verify that it wasn't overwritten in the update process:
+#
+#   ./stm32loader.py -r -a 0x803f800 -l 2048 -p /dev/ttymxc1 readback.bin
+#   hexdump readback.bin
+#
+# if it's not all 0xff's you're in good shape!
+
+#Uncomment the RCTX line
+sed -i 's/.\(RCTX*.\)/\1/g' /etc/inittab
+
+#start the stm32 process back up
+init q

sololink/config/checknet

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+# Verify network device exists, and reboot if it does not
+
+# This is a workaround for the case where the PCIe phy does not initialize,
+# which happens a few percent of the time. The user-facing effect is that
+# startup takes about 20 seconds longer if the phy does not initialize.
+#
+# It would be better to do this from the initrd.
+#
+# It would be better still to fix the PCIe driver to reset and try again (or
+# something). Simple fixes (e.g. increasing the training time) did not work.
+# The imx6 PCIe driver in the 3.14.38 BSP is considerably different from the
+# one in the 3.10.17 BSP, so perhaps it will be fixed when we update.
+
+kmsg() {
+  echo "$@" > /dev/kmsg
+}
+
+# wlan0 exists on both solo and controller
+# (although normally it is not used on controller)
+kmsg -n "Checking wlan0... "
+
+if [ ! -e /sys/class/net/wlan0 ]; then
+  kmsg "NOT FOUND"
+  # all logs except boot have been rotated at this point;
+  # stop and rotate it keep them all in sync
+  /etc/init.d/bootlogd stop
+  shutdown -r now
+  # don't let any more startup scripts start
+  sleep 60
+else
+  kmsg "OK"
+fi

sololink/config/configinit

@@ -0,0 +1,285 @@
+#!/bin/bash
+
+# SoloLink Configuration file validate/restore
+#
+# Called at init time before any config files are used via an entry in
+# /etc/rcS.d.
+
+cd /etc
+
+verbose=false
+
+# arguments
+# -v verbose
+until [ -z "$1" ]; do
+  case $1 in
+    -v) verbose=true;;
+  esac
+  shift
+done
+
+dmsg() {
+  if ${verbose}; then echo "$@"; fi
+}
+
+# Test mode: Ubuntu md5sum is a bit different from busybox md5sum
+# /usr/bin/md5sum is a symlink if it points to busybox
+if [ -h `which md5sum` ]; then
+  # assume busybox
+  # not test mode
+  MD5SUM="md5sum -cs"
+  ETC="/etc"
+  ROFS="/mnt/rootfs.ro"
+  kmsg() {
+    echo "$@" > /dev/kmsg
+  }
+else
+  # assume not busybox; correct for ubuntu but not tested on os-x
+  # test mode
+  MD5SUM="md5sum -c --status"
+  ETC="./etc"
+  ROFS="./rootfs.ro"
+  kmsg() {
+    echo "$@"
+  }
+fi
+
+kmsg -n "Checking SoloLink configuration... "
+dmsg ""
+
+config_names="hostapd wpa_supplicant sololink shotmanager"
+
+# For each file "file" in ${config_names}:
+#
+# A new FS contains only "file.orig"
+#
+# First boot:
+#   1. file.conf does not exist
+#   2. file.back does not exist
+#   3. copy file.orig to file.base (used for upgrade merge)
+#   4. copy file.orig to file.conf
+#   5. create file.conf.md5
+#
+# Subsequent boots:
+#   1. file.conf exists
+#   2. file.conf.md5 is good
+#
+# Program modification of a config file (not in this script):
+#   1. copy file.conf to file.back
+#   2. create file.back.md5
+#   3. modify file.conf
+#   4. create file.conf.md5
+#
+# Manual modification of a config file (e.g. via console or ssh):
+#   1. edit file.conf
+#   2. md5sum <full-path-to-file.conf> > <full-path-to-file.conf.md5>
+#
+# During the modification process, a shutdown can result in any of the
+# following (numbers match completion of steps above):
+#   1. file.conf valid, file.back invalid (file.conf is used)
+#   2. file.conf valid, file.back valid (file.conf used)
+#   3. file.conf invalid, file.back valid (file.conf rollback)
+#   4. file.conf valid, file.back valid (file.conf used)
+#
+# Example recovery:
+#   1. file.conf exists
+#   2. file.conf.md5 is not good
+#   3. file.back exists
+#   4. file.back.md5 is good
+#   5. copy file.back to file.conf
+#   6. create file.conf.md5
+#
+# Example upgrade:
+#   1. file.conf exists
+#   2. file.conf.md5 is good
+#   3. file.orig.md5 exists
+#   4. file.orig.md5 is not good (because file.orig changed)
+#   5. patch = file.base -> file.conf; previous changes (e.g. setting ssid)
+#   6. apply patch to new file.orig, creating file.conf
+#   7. create file.conf.md5
+#   8. create file.orig.md5
+#   9. copy file.orig to file.base
+#  10. create file.base.md5
+#
+# Example upgrade (merge fails):
+#   1. file.conf exists
+#   2. file.conf.md5 is good
+#   3. file.orig.md5 exists
+#   4. file.orig.md5 is not good (because file.orig changed)
+#   5. file.base is not good (does not exist)
+#   6. copy file.orig to file.conf
+#   7. create file.conf.md5
+#   8. create file.orig.md5
+#   9. copy file.orig to file.base
+#  10. create file.base.md5
+
+# Return true if file exists, md5 exists, and the md5 is correct
+valid() {
+  [ -f ${1} ] && [ -f ${1}.md5 ] && ${MD5SUM} ${1}.md5
+}
+
+# Return true if orig.md5 does not exist, or it exists but is not correct
+upgraded() {
+  [ ! -f ${1}.orig.md5 ] || ! ${MD5SUM} ${1}.orig.md5
+}
+
+# Create backup of .conf
+do_backup() {
+  dmsg "backup ${1}.conf -> ${1}.back"
+  cp ${1}.conf ${1}.back
+  md5sum `realpath ${1}.back` > ${1}.back.md5
+}
+
+# Restore one file to another, i.e. .back or .orig to .conf
+do_restore() {
+  dmsg "copy ${1} -> ${2}"
+  cp ${1} ${2}
+  md5sum `realpath ${2}` > ${2}.md5
+}
+
+# Note that the busybox 'patch' we are using is relatively limited:
+#   patch [-p NUM] [-i DIFF] [-R] [-N]
+#           -p NUM  Strip NUM leading components from file names
+#           -i DIFF Read DIFF instead of stdin
+#           -R      Reverse patch
+#           -N      Ignore already applied patches
+#
+# Busybox patch's return status is not documented. GNU patch's return is this:
+#   patch's exit status is 0 if all hunks are applied successfully, 1 if some
+#   hunks cannot be applied or there were  merge  conflicts, and 2 if there is
+#   more serious trouble.
+# We assume here it is zero on success, nonzero on any failure.
+do_patch() {
+  #dmsg "patch ${1}.conf"
+  patch ${1}.conf < ${1}.patch
+  # patch's return code is this function's return code
+}
+
+# Merge changes.
+#
+# config.base is a copy of the previous config.orig (before this update)
+# config.conf is that, plus any changes the user has made
+# config.orig is the _new_ config for this update
+#
+# 1. config.base -> config.conf is what has changed from the previous
+#    config.orig to the previous config.conf. Create a patch from that.
+#
+# 2. Attempt to apply the patch to the _new_ config.orig.
+#
+#    a. If the patch applies cleanly, the new config.conf is the new
+#       config.orig plus patch.
+#
+#    b. If the patch does not apply cleanly, the new config.conf is just
+#       copied from the new config.orig (changes are lost).
+#
+# Argument is the config file name root, e.g. "hostapd" means we are working
+# with hostapd.base, hostapd.conf, and hostapd.orig.
+#
+do_merge() {
+  dmsg "merge ${1}"
+
+  # config.conf is known to be valid at this point
+
+  if valid ${1}.base; then
+    # config.base is valid; try to merge changes into new config
+
+    # config.patch is the user's changes in the pre-upgrade system
+    diff -au ${1}.base ${1}.conf > ${1}.patch
+
+    # start with upgraded config.conf...
+    do_backup ${1}
+    cp ${1}.orig ${1}.conf
+
+    # apply user's changes
+    if ! do_patch ${1}; then
+      # some hunks failed - partial patching seems confusing, so revert it all
+      dmsg "patch ${1}.conf failed"
+      dmsg "copy ${1}.orig -> ${1}.conf"
+      cp ${1}.orig ${1}.conf
+    fi
+
+    # .conf is good now, either updated or just copied
+    md5sum `realpath ${1}.conf` > ${1}.conf.md5
+    rm -f ${1}.patch ${1}.back* ${1}.conf.orig
+
+  else
+    # config.base is not valid, can't merge
+    dmsg "${1}.base not valid; can't merge"
+    dmsg "copy ${1}.orig -> ${1}.conf"
+    cp ${1}.orig ${1}.conf
+    md5sum `realpath ${1}.conf` > ${1}.conf.md5
+  fi
+
+  # this is how we detect when the next upgrade happens
+  md5sum `realpath ${1}.orig` > ${1}.orig.md5
+
+  # new config.base will be used in the next upgrade
+  cp ${1}.orig ${1}.base
+  md5sum `realpath ${1}.base` > ${1}.base.md5
+}
+
+# Loop over all config files, validating and perhaps restoring each one
+for config_name in ${config_names}; do
+  config=${ETC}/${config_name}
+
+  # skip any that are not in the read-only FS (e.g. shotmanager on controller)
+  if [ ! -f ${ROFS}/etc/${config_name}.orig ]; then
+    continue
+  fi
+
+  dmsg ""
+  dmsg "${config_name}... "
+
+  if valid ${config}.conf; then
+    dmsg "${config}.conf is valid"
+
+    if upgraded ${config}; then
+      kmsg -n "merging ${config}... "
+      dmsg ""
+      # typical just after an upgrade if .orig file changes
+      do_merge ${config}
+    else
+      # typical startup
+      dmsg "done"
+    fi
+
+  elif valid ${config}.back; then
+    dmsg "${config}.back is valid"
+
+    # previous shutdown corrupted the .conf file
+    kmsg -n "restoring ${config}.conf... "
+    dmsg ""
+    do_restore ${config}.back ${config}.conf
+    if upgraded ${config}; then
+      kmsg -n "merging ${config}... "
+      dmsg ""
+      # edge case: previous shutdown prepared update and corrupted .conf file
+      do_merge ${config}
+    else
+      dmsg "done"
+    fi
+
+  else
+    dmsg "neither ${config}.conf nor ${config}.back is valid"
+
+    # typical on first boot
+    kmsg -n "initializing ${config}.conf... "
+    dmsg ""
+    if [ ! -f ${config}.orig ]; then
+      kmsg -n "${config}.orig not found; retrieving from ${ROFS}/etc/${config_name}.orig... "
+      dmsg ""
+      cp ${ROFS}/etc/${config_name}.orig ${config}.orig
+    fi
+    dmsg "copy ${config}.orig -> ${config}.base"
+    cp ${config}.orig ${config}.base
+    md5sum `realpath ${config}.base` > ${config}.base.md5
+    do_restore ${config}.orig ${config}.conf
+    md5sum `realpath ${config}.orig` > ${config}.orig.md5
+    dmsg "done"
+
+  fi
+
+done
+
+dmsg ""
+kmsg "OK"

sololink/config/logrotate-sololink.conf.controller

@@ -0,0 +1,23 @@
+# see "man logrotate" for details
+# rotate logs, keeping 20 versions
+
+# specifying 'create' makes it so the log file is always there, even if
+# nothing writes to it on a particular run, so on the next rotation, the empty
+# file is rotated along with all the others, keeping the .N extensions in sync
+# (e.g. so all the .2 files are from the same run).
+
+# specifying 'missingok' is needed for the very first run when there are no
+# log files to rotate.
+
+create
+missingok
+rotate 20
+nodateext
+
+/log/3dr-top.log { }
+/log/3dr-solo.log { }
+/log/3dr-stm32.log { }
+/log/3dr-video.log { }
+/log/3dr-wifi.log { }
+/log/3dr-temp.log { }
+/log/pkt_delays.csv { }

sololink/config/logrotate-sololink.conf.solo

@@ -0,0 +1,25 @@
+# see "man logrotate" for details
+# rotate logs, keeping 20 versions
+
+# specifying 'create' makes it so the log file is always there, even if
+# nothing writes to it on a particular run, so on the next rotation, the empty
+# file is rotated along with all the others, keeping the .N extensions in sync
+# (e.g. so all the .2 files are from the same run).
+
+# specifying 'missingok' is needed for the very first run when there are no
+# log files to rotate.
+
+create
+missingok
+rotate 20
+nodateext
+
+/log/3dr-top.log { }
+/log/3dr-solo.log { }
+/log/3dr-rc.log { }
+/log/3dr-telem.log { }
+/log/3dr-wifi.log { }
+/log/3dr-video.log { }
+/log/3dr-temp.log { }
+/log/shotlog.log { }
+/log/3dr-accessorykit.log { }

sololink/config/max_dgram_qlen

@@ -0,0 +1,3 @@
+#! /bin/sh
+echo "100" > /proc/sys/net/unix/max_dgram_qlen
+echo "max_dgram_qlen=`cat /proc/sys/net/unix/max_dgram_qlen`"

sololink/config/sololink

@@ -0,0 +1,5 @@
+# This file goes in /etc/profile.d/ and is sourced by /etc/profile at boot time
+# Contents are created by sololink recipe, and should end up looking something
+# like this:
+# export SOLOLINK_CONFIG_DIR=/etc
+# export SOLOLINK_LOG_DIR=/log

sololink/config/sololink.orig

@@ -0,0 +1,191 @@
+[solo]
+
+artooIp=10.1.1.1
+
+soloIp=10.1.1.10
+
+# Address to which RC packets are sent
+rcDestPort=5005
+
+# Address to which 'sysinfo' packet for the STM32 is sent
+sysDestIp=%(artooIp)s
+sysDestPort=5012
+
+# Port to which 'pair request' packet for the STM32 is sent
+pairReqDestPort=5013
+
+# Port to which 'pair result' packet for the STM32 is sent
+pairResDestPort=5014
+
+# Port to which MAVLink packets for the STM32 are sent
+mavDestPort=5015
+
+# Port to connect to for button events (TCP)
+buttonEventPort=5016
+
+# Port to send button function config messages to
+buttonFunctionConfigDestPort=5017
+
+# Port to send set shot info messages to
+setShotInfoDestPort=5018
+
+# Port to send updater messages to
+updaterDestPort=5019
+
+# Port to which MAVLink packets are sent for all external systems
+telemDestPort=14550
+
+# TCP port where app server listens for connections
+appServerPort=5502
+
+# File where app_server saves connect app's IP address
+appAddressFile=/var/run/solo_app.ip
+
+# Artoo's serial ports
+
+# Console is /dev/ttymxc0
+
+# STM32
+stm32Dev=/dev/ttymxc1
+stm32Baud=115200
+
+# Solo's serial ports
+
+# Console is /dev/ttymxc0
+
+# Pixhawk telemetry
+telemDev=/dev/ttymxc1
+telemBaud=921600
+telemFlow=True
+
+# Telemetry logging control
+telemLogGap=1000000
+telemLogDelayMax=100000
+#telemLogDelayFile=/tmp/pkt_delays.csv
+
+# Pixhawk RC
+rcDsmDev=/dev/ttymxc2
+rcDsmBaud=115200
+
+# IP addresses from which Solo accepts RC packets
+rcSourceIps=10.1.1.1,127.0.0.1
+
+# Set system time from GPS when available
+useGpsTime=True
+
+# Throttle PWM mapping
+pwmInMinThrottle=1000
+pwmInMaxThrottle=2000
+pwmOutMinThrottle=1000
+pwmOutMaxThrottle=1900
+
+# Rc timeout max
+rcTimeoutUS=400000
+
+# Telemetry display units (metric or imperial)
+uiUnits=metric
+
+[pairing]
+user_confirmation_timeout = 30.0
+controller_link_port = 5501
+wifi_connect_timeout = 5.0
+connect_request_interval = 1.0
+connect_ack_timeout = 0.5
+solo_address_file = /var/run/solo.ip
+button_filename = /dev/input/event0
+
+[net]
+ApEnable=True
+StationEnable=False
+
+[loggers]
+keys=root,stm32,pix,pair,net,app,tlm,shot
+
+[handlers]
+keys=consoleHandler,sysLogHandler,sysLog2Handler
+
+[formatters]
+keys=simpleFormatter,syslogFormatter
+
+[logger_root]
+level=INFO
+handlers=consoleHandler
+
+[logger_stm32]
+level=INFO
+handlers=sysLogHandler
+qualname=stm32
+propagate=0
+
+[logger_pix]
+level=INFO
+handlers=sysLogHandler
+qualname=pix
+propagate=0
+
+[logger_pair]
+level=INFO
+handlers=sysLogHandler
+qualname=pair
+propagate=0
+
+[logger_net]
+level=INFO
+handlers=sysLogHandler
+qualname=net
+propagate=0
+
+[logger_app]
+level=INFO
+handlers=sysLogHandler
+qualname=app
+propagate=0
+
+[logger_tlm]
+level=INFO
+handlers=sysLogHandler
+qualname=tlm
+propagate=0
+
+[logger_shot]
+level=INFO
+handlers=sysLog2Handler
+qualname=shot
+propagate=0
+
+[handler_consoleHandler]
+class=StreamHandler
+level=ERROR
+formatter=simpleFormatter
+args=(sys.stdout,)
+
+[handler_sysLogHandler]
+class=handlers.SysLogHandler
+level=DEBUG
+formatter=syslogFormatter
+args=("/dev/log", handlers.SysLogHandler.LOG_LOCAL1)
+
+[handler_sysLog2Handler]
+class=handlers.SysLogHandler
+level=DEBUG
+formatter=syslogFormatter
+args=("/dev/log", handlers.SysLogHandler.LOG_LOCAL2)
+
+[formatter_simpleFormatter]
+format=%(asctime)s %(name)-4s %(levelname)-8s %(message)s
+datefmt=
+
+[formatter_syslogFormatter]
+format=%(name)s: %(message)s
+datefmt=
+
+[video]
+videoMinFR=24
+videoMaxFR=24
+videoMinBR=800000
+videoMaxBR=1800000
+videoFRStep=5
+videoBRStep=100000
+varStreamRes=True
+cropRecordRes=True
+

sololink/config/sololink_config_sample

@@ -0,0 +1,28 @@
+#!/bin/bash
+
+# run_cmd prints a command, then the command's output, then 'ok' or 'error',
+# depending on the command's exit status, then a blank line.
+#
+# All text after the command and before the 'ok' or 'error' is what the
+# command sends to stdout.
+run_cmd() {
+echo $1
+$1
+if [ $? -eq 0 ]; then echo "ok"; else echo "error"; fi
+echo
+}
+
+for SSH in "ssh root@10.1.1.1" "ssh root@10.1.1.10"; do
+
+run_cmd "$SSH sololink_config --get-version"
+run_cmd "$SSH sololink_config --get-version sololink"
+run_cmd "$SSH sololink_config --get-version artoo"
+run_cmd "$SSH sololink_config --get-version pixhawk"
+run_cmd "$SSH sololink_config --get-version golden"
+run_cmd "$SSH sololink_config --get-version all"
+run_cmd "$SSH sololink_config --get-wifi-ssid"
+run_cmd "$SSH sololink_config --get-wifi-password"
+run_cmd "$SSH sololink_config --get-image"
+run_cmd "$SSH sololink_config --get-pairing"
+
+done

sololink/config/sololink_config_test

@@ -0,0 +1,689 @@
+#!/bin/bash
+
+# runs on ubuntu-14.04
+# does not run on osx
+
+if [ -z ${TESTHOSTNAME} ]; then
+  echo "ERROR: set TESTHOSTNAME"
+  echo "       TESTHOSTNAME=3dr_solo ./sololink_config_test"
+  echo "       TESTHOSTNAME=3dr_controller ./sololink_config_test"
+  exit 1
+fi
+
+export SOLOLINK_CONFIG_TEST=1
+export ROOT=./test_${TESTHOSTNAME}
+
+error=0
+
+if ! ./sololink_config --pass; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+fi
+
+if ./sololink_config --fail; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+fi
+
+### WIFI SSID
+
+# backup, ignoring errors
+cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1
+cp ${ROOT}/etc/hostapd.conf hostapd.save > /dev/null 2>&1
+
+# get to initial state
+./sololink_config --reset-wifi-settings
+
+echo -n "wifi: set ssid; not set ... "
+x=`./sololink_config --set-wifi-ssid SoloLink_one`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "SoloLink_one" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set ssid; already set ... "
+x=`./sololink_config --set-wifi-ssid SoloLink_two`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "SoloLink_two" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set ssid; too short ... "
+x=`./sololink_config --set-wifi-ssid 1234567`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set ssid; not SoloLink_ ... "
+x=`./sololink_config --set-wifi-ssid 12345678`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set ssid; max length ... "
+xx="SoloLink_12345678901234567890123"
+x=`./sololink_config --set-wifi-ssid $xx`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "$xx" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set ssid; too long ... "
+x=`./sololink_config --set-wifi-ssid SoloLink_123456789012345678901234`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+# restore
+if [ -f wpa_supplicant.save ]; then
+  mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf
+fi
+if [ -f hostapd.save ]; then
+  mv hostapd.save ${ROOT}/etc/hostapd.conf
+fi
+
+### WIFI PASSWORD
+
+# backup, ignoring errors
+cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1
+cp ${ROOT}/etc/hostapd.conf hostapd.save > /dev/null 2>&1
+
+# get to initial state
+./sololink_config --reset-wifi-settings
+
+echo -n "wifi: set password; not set ... "
+x=`./sololink_config --set-wifi-password 12345678`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "12345678" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set password; already set ... "
+x=`./sololink_config --set-wifi-password 87654321`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "87654321" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set password; with space ... "
+x=`./sololink_config --set-wifi-password "ABCD EFGH"`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "ABCD EFGH" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set password; max length ... "
+xx=123456789012345678901234567890123456789012345678901234567890123
+x=`./sololink_config --set-wifi-password $xx`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "$xx" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set password; too short ... "
+x=`./sololink_config --set-wifi-password 1234567`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "wifi: set password; too long ... "
+xx=1234567890123456789012345678901234567890123456789012345678901234
+x=`./sololink_config --set-wifi-password $xx`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+# restore
+if [ -f wpa_supplicant.save ]; then
+  mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf
+fi
+if [ -f hostapd.save ]; then
+  mv hostapd.save ${ROOT}/etc/hostapd.conf
+fi
+
+### PAIR/UNPAIR
+
+# backup, ignoring errors
+cp ${ROOT}/etc/wpa_supplicant.conf wpa_supplicant.save > /dev/null 2>&1
+cp ${ROOT}/log/3dr-pairing.conf 3dr-pairing.save > /dev/null 2>&1
+
+echo -n "pair: set pairing; unpair ... "
+x=`./sololink_config --set-pairing`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+    if grep -q "network=" ${ROOT}/etc/wpa_supplicant.conf; then
+      echo "ERROR at line ${LINENO}"
+      error=1
+    else
+      echo "OK"
+    fi
+  else
+    if [ -f ${ROOT}/log/3dr-pairing.conf ]; then
+      echo "ERROR at line ${LINENO}"
+      error=1
+    else
+      echo "OK"
+    fi
+  fi
+fi
+
+echo -n "pair: get pairing; when unpaired ... "
+pair=`./sololink_config --get-pairing`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "pair: set pairing; not paired ... "
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  peer="SoloLink_ABC"
+elif [ "${TESTHOSTNAME}" == "3dr_controller" ]; then
+  peer="12:34:56:78:90:ab"
+fi
+pair=`./sololink_config --set-pairing ${peer}`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+    if ! grep -q "ssid=\"${peer}\"" ${ROOT}/etc/wpa_supplicant.conf; then
+      echo "ERROR at line ${LINENO}"
+      cat ${ROOT}/etc/wpa_supplicant.conf
+      error=1
+    else
+      echo "OK"
+    fi
+  else
+    if [ ! -f ${ROOT}/log/3dr-pairing.conf ]; then
+      echo "ERROR at line ${LINENO}"
+      error=1
+    else
+      echo "OK"
+    fi
+  fi
+fi
+
+echo -n "pair: set pairing; already paired ... "
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  peer="SoloLink_123"
+elif [ "${TESTHOSTNAME}" == "3dr_controller" ]; then
+  peer="cd:EF:ab:11:22:9A"
+fi
+x=`./sololink_config --set-pairing ${peer}`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+    if ! grep -q "ssid=\"${peer}\"" ${ROOT}/etc/wpa_supplicant.conf; then
+      echo "ERROR at line ${LINENO}"
+      cat ${ROOT}/etc/wpa_supplicant.conf
+      error=1
+    else
+      echo "OK"
+    fi
+  else
+    if [ ! -f ${ROOT}/log/3dr-pairing.conf ]; then
+      echo "ERROR at line ${LINENO}"
+      error=1
+    else
+      echo "OK"
+    fi
+  fi
+fi
+
+# restore
+if [ -f wpa_supplicant.save ]; then
+  mv wpa_supplicant.save ${ROOT}/etc/wpa_supplicant.conf
+fi
+if [ -f 3dr-pairing.save ]; then
+  mv 3dr-pairing.save ${ROOT}/log/3dr-pairing.conf
+else
+  rm -f ${ROOT}/log/3dr-pairing.conf
+fi
+
+### UPDATE
+
+echo -n "update: factory reset ... "
+x=`./sololink_config --factory-reset`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ ! -f ${ROOT}/log/updates/FACTORYRESET ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "update: settings reset ... "
+x=`./sololink_config --settings-reset`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ ! -f ${ROOT}/log/updates/RESETSETTINGS ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+dir_empty() {
+  [ -d ${1} ] && [ `find ${1} | wc -l` -eq 1 ]
+}
+
+echo -n "update: prepare artoo ... "
+x=`./sololink_config --update-prepare artoo`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+else
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  elif ! dir_empty ${ROOT}/firmware; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+fi
+
+echo -n "update: apply artoo ... "
+x=`./sololink_config --update-apply artoo`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+else
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+fi
+
+echo -n "update: prepare pixhawk ... "
+x=`./sololink_config --update-prepare pixhawk`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  elif ! dir_empty ${ROOT}/firmware; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+else
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+fi
+
+echo -n "update: apply pixhawk ... "
+x=`./sololink_config --update-apply pixhawk`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+else
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+fi
+
+echo -n "update: prepare sololink ... "
+x=`./sololink_config --update-prepare sololink`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif ! dir_empty ${ROOT}/log/updates; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+echo -n "update: apply sololink; correct file ... "
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  updatefile="3dr-solo-1234.tar.gz"
+else
+  updatefile="3dr-controller-1234.tar.gz"
+fi
+touch ${ROOT}/log/updates/${updatefile}
+pushd ${ROOT}/log/updates > /dev/null 2>&1
+md5sum ${updatefile} > ${updatefile}.md5
+popd > /dev/null 2>&1
+x=`./sololink_config --update-apply sololink`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ ! -f ${ROOT}/log/updates/UPDATE ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+rm ${ROOT}/log/updates/*
+
+echo -n "update: apply sololink; incorrect file ... "
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  updatefile="3dr-controller-1234.tar.gz"
+else
+  updatefile="3dr-solo-1234.tar.gz"
+fi
+touch ${ROOT}/log/updates/${updatefile}
+pushd ${ROOT}/log/updates > /dev/null 2>&1
+md5sum ${updatefile} > ${updatefile}.md5
+popd > /dev/null 2>&1
+x=`./sololink_config --update-apply sololink`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+rm ${ROOT}/log/updates/${updatefile}*
+
+echo -n "update: apply sololink; bad md5 ... "
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  updatefile="3dr-solo-1234.tar.gz"
+else
+  updatefile="3dr-controller-1234.tar.gz"
+fi
+touch ${ROOT}/log/updates/${updatefile}
+pushd ${ROOT}/log/updates > /dev/null 2>&1
+md5sum ${updatefile} > ${updatefile}.md5
+md5sum ${updatefile}.md5 > ${updatefile}.md5
+popd > /dev/null 2>&1
+x=`./sololink_config --update-apply sololink`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+rm ${ROOT}/log/updates/${updatefile}*
+
+echo -n "update: apply sololink; missing file ... "
+x=`./sololink_config --update-apply sololink`
+if [ $? -eq 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+### VERSION
+
+echo -n "version: script ... "
+x=`./sololink_config --get-version`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "${x} ... OK"
+fi
+
+echo -n "version: sololink ... "
+x=`./sololink_config --get-version sololink`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}: ${x}"
+  error=1
+else
+  echo "${x} ... OK"
+fi
+
+echo -n "version: artoo ... "
+x=`./sololink_config --get-version artoo`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}: ${x}"
+    error=1
+  else
+    echo "${x} ... OK"
+  fi
+else
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}: ${x}"
+    error=1
+  else
+    echo "${x} ... OK"
+  fi
+fi
+
+echo -n "version: pixhawk ... "
+x=`./sololink_config --get-version pixhawk`
+r=$?
+if [ "${TESTHOSTNAME}" == "3dr_solo" ]; then
+  if [ ${r} -ne 0 ]; then
+    echo "ERROR at line ${LINENO}: ${x}"
+    error=1
+  else
+    echo "${x} ... OK"
+  fi
+else
+  if [ ${r} -eq 0 ]; then
+    echo "ERROR at line ${LINENO}: ${x}"
+    error=1
+  else
+    echo "${x} ... OK"
+  fi
+fi
+
+### GET/SET CONFIG
+
+# backup, ignoring errors
+cp ${ROOT}/etc/sololink.conf sololink.save > /dev/null 2>&1
+cp ${ROOT}/etc/sololink.conf.md5 sololink.save.md5 > /dev/null 2>&1
+
+# append test variables
+cat >> ${ROOT}/etc/sololink.conf <<TESTVARS
+var0=val0
+ var1=val1
+var2 =val2
+var3= val3
+var4 = val4
+	var5=val5
+var6	=val6
+var7=	val7
+var8	=	val8
+	 var9  =	val9
+TESTVARS
+
+check_get_config() {
+  echo -n "get-config: ${1}=${2} ... "
+  x=`./sololink_config --get-config ${1}`
+  if [ $? -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  elif [ "$x" != "${2}" ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+}
+
+check_set_config() {
+  echo -n "set-config: ${1}=${2} ... "
+  x=`./sololink_config --set-config ${1} ${2}`
+  if [ $? -ne 0 ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  elif [ "$x" != "${2}" ]; then
+    echo "ERROR at line ${LINENO}"
+    error=1
+  else
+    echo "OK"
+  fi
+}
+
+# get, does not exist
+echo -n "get-config: get nonexistant variable ... "
+x=`./sololink_config --get-config qwerty`
+if [ $? -ne 1 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "qwerty not set" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+# set, does not exist
+echo -n "set-config: set nonexistant variable ... "
+x=`./sololink_config --set-config qwerty asdf`
+if [ $? -ne 1 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "qwerty not set" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+# set, does not exist, add new
+echo -n "set-config: set nonexistant variable with add ... "
+x=`./sololink_config --set-config qwerty asdf --add`
+if [ $? -ne 0 ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+elif [ "$x" != "asdf" ]; then
+  echo "ERROR at line ${LINENO}"
+  error=1
+else
+  echo "OK"
+fi
+
+# get
+check_get_config var0 val0
+check_get_config var1 val1
+check_get_config var2 val2
+check_get_config var3 val3
+check_get_config var4 val4
+check_get_config var5 val5
+check_get_config var6 val6
+check_get_config var7 val7
+check_get_config var8 val8
+check_get_config var9 val9
+
+# set
+check_set_config var0 new0
+check_set_config var1 new1
+check_set_config var2 new2
+check_set_config var3 new3
+check_set_config var4 new4
+check_set_config var5 new5
+check_set_config var6 new6
+check_set_config var7 new7
+check_set_config var8 new8
+check_set_config var9 new9
+
+# get
+check_get_config var0 new0
+check_get_config var1 new1
+check_get_config var2 new2
+check_get_config var3 new3
+check_get_config var4 new4
+check_get_config var5 new5
+check_get_config var6 new6
+check_get_config var7 new7
+check_get_config var8 new8
+check_get_config var9 new9
+
+# restore
+if [ -f sololink.save ]; then
+  mv sololink.save ${ROOT}/etc/sololink.conf
+fi
+if [ -f sololink.save.md5 ]; then
+  mv sololink.save.md5 ${ROOT}/etc/sololink.conf.md5
+fi
+
+###
+
+if [ $error -eq 0 ]; then
+  echo ALL PASSED
+else
+  echo ERRORS
+fi

sololink/config/syslog.conf.busybox.controller

@@ -0,0 +1,9 @@
+# Configuration file for busybox's syslogd utility
+local0.info	/log/3dr-stm32.log
+local1.info	/log/3dr-solo.log
+local2.info	/log/3dr-wifi.log
+local3.info	/log/3dr-video.log
+
+local5.info	/log/3dr-temp.log
+
+kern,user.*	/log/kern.log

sololink/config/syslog.conf.busybox.solo

@@ -0,0 +1,11 @@
+# Configuration file for busybox's syslogd utility
+local0.info	/log/3dr-rc.log
+local1.info	/log/3dr-solo.log
+local2.info	/log/shotlog.log
+local3.info	/log/3dr-wifi.log
+local4.info	/log/3dr-telem.log
+local5.info	/log/3dr-temp.log
+local6.info	/log/3dr-video.log
+local7.info	/log/3dr-accessorykit.log
+
+kern,user.*	/log/kern.log

sololink/config/test_3dr_README

@@ -0,0 +1,2 @@
+The directories test_3dr_controller and test_3dr_solo are used by the test
+script sololink_config_test.

sololink/config/test_3dr_controller/STM_VERSION

@@ -0,0 +1 @@
+0.6.9

sololink/config/test_3dr_controller/VERSION

@@ -0,0 +1 @@
+0.6.5

sololink/config/test_3dr_controller/etc/hostapd.conf.md5

@@ -0,0 +1 @@
+a46c206ffafef83c91cebf497ea785b5  ./test_3dr_controller/etc/hostapd.conf

sololink/config/test_3dr_solo/PIX_VERSION

@@ -0,0 +1 @@
+0.0.28

sololink/config/test_3dr_solo/VERSION

@@ -0,0 +1 @@
+0.6.5

sololink/config/test_3dr_solo/etc/wpa_supplicant.conf

@@ -0,0 +1,6 @@
+ctrl_interface=/var/run/wpa_supplicant
+ctrl_interface_group=0
+update_config=1
+device_name=Solo
+manufacturer=3D Robotics
+model_name=Solo

sololink/config/test_3dr_solo/etc/wpa_supplicant.conf.md5

@@ -0,0 +1 @@
+132d9621948cfb385c5975b51dec4758  ./test_3dr_solo/etc/wpa_supplicant.conf

sololink/config/test_3dr_solo/mnt/rootfs.ro/etc/wpa_supplicant.orig

@@ -0,0 +1,6 @@
+ctrl_interface=/var/run/wpa_supplicant
+ctrl_interface_group=0
+update_config=1
+device_name=Solo
+manufacturer=3D Robotics
+model_name=Solo

sololink/config/test_configinit

@@ -0,0 +1,244 @@
+#!/bin/sh
+
+verbose=
+
+# arguments
+# -v verbose
+until [ -z "$1" ]; do
+  case "$1" in
+    -v) verbose="-v" ;;
+  esac
+  shift
+done
+
+# This runs on Ubuntu
+MD5SUM='md5sum -c --status'
+
+config_names="hostapd wpa_supplicant sololink"
+
+valid() {
+  [ -f ${1} ] && [ -f ${1}.md5 ] && ${MD5SUM} ${1}.md5
+}
+
+assert_valid() {
+  if ! valid ${1}; then
+    echo "##### TEST FAILED ##### (valid ${1})"
+  fi
+}
+
+assert_same() {
+  if ! cmp ${1} ${2}; then
+    echo "##### TEST FAILED ##### (cmp ${1} ${2})"
+  fi
+}
+
+test_first_boot() {
+  echo
+  echo "##### test_first_boot starting"
+  echo
+
+  # delete all
+  mkdir -p etc/
+  rm -f etc/*
+
+  # only .orig files exist
+  for config in ${config_names}; do
+    cp rootfs.ro/etc/${config}.orig etc/${config}.orig
+    chmod +w etc/${config}.orig
+  done
+
+  ./configinit ${verbose}
+
+  # should have:
+  # * .base, .base.md5
+  # * .conf, .conf.md5
+  # * .orig, .orig.md5
+  # * .base, .conf, .orig should all be the same
+  # * all .md5 should be good
+  for config in ${config_names}; do
+    assert_valid etc/${config}.base
+    assert_valid etc/${config}.conf
+    assert_valid etc/${config}.orig
+    assert_same etc/${config}.orig etc/${config}.base
+    assert_same etc/${config}.orig etc/${config}.conf
+  done
+
+  echo
+  echo "##### test_first_boot complete"
+  echo
+}
+
+test_normal_boot() {
+  echo
+  echo "##### test_normal_boot starting"
+  echo
+
+  # delete all
+  mkdir -p etc/
+  rm -f etc/*
+
+  # .orig, .base, and .conf files exist
+  for config in ${config_names}; do
+    cp rootfs.ro/etc/${config}.orig etc/${config}.orig
+    chmod +w etc/${config}.orig
+    md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5
+    cp etc/${config}.orig etc/${config}.base
+    md5sum `realpath etc/${config}.base` > etc/${config}.base.md5
+    cp etc/${config}.orig etc/${config}.conf
+    # .conf is normally changed from .orig
+    echo "# added line!" >> etc/${config}.conf
+    md5sum `realpath etc/${config}.conf` > etc/${config}.conf.md5
+  done
+
+  ./configinit ${verbose}
+
+  # should have:
+  # * .base, .base.md5
+  # * .conf, .conf.md5
+  # * .orig, .orig.md5
+  # * .base and .orig should be the same
+  # * all .md5 should be good
+  for config in ${config_names}; do
+    assert_valid etc/${config}.base
+    assert_valid etc/${config}.conf
+    assert_valid etc/${config}.orig
+    assert_same etc/${config}.orig etc/${config}.base
+  done
+
+  echo
+  echo "##### test_normal_boot complete"
+  echo
+}
+
+test_invalid_conf() {
+  echo
+  echo "##### test_invalid_conf starting"
+  echo
+
+  # delete all
+  mkdir -p etc/
+  rm -f etc/*
+
+  # .orig, .base, and .conf files exist
+  for config in ${config_names}; do
+    cp rootfs.ro/etc/${config}.orig etc/${config}.orig
+    chmod +w etc/${config}.orig
+    md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5
+    cp etc/${config}.orig etc/${config}.base
+    md5sum `realpath etc/${config}.base` > etc/${config}.base.md5
+    cp etc/${config}.orig etc/${config}.conf
+    # incomplete update of .conf
+    cp etc/${config}.conf etc/${config}.back
+    md5sum `realpath etc/${config}.back` > etc/${config}.back.md5
+    echo "# added line!" >> etc/${config}.conf
+    # .conf.md5 is now invalid
+  done
+
+  ./configinit ${verbose}
+
+  # should have:
+  # * .base, .base.md5
+  # * .conf, .conf.md5
+  # * .orig, .orig.md5
+  # * .base and .orig should be the same
+  # * all .md5 should be good
+  for config in ${config_names}; do
+    assert_valid etc/${config}.base
+    assert_valid etc/${config}.conf
+    assert_valid etc/${config}.orig
+    assert_same etc/${config}.orig etc/${config}.base
+  done
+
+  echo
+  echo "##### test_invalid_conf complete"
+  echo
+}
+
+test_update() {
+  echo
+  echo "##### test_update starting"
+  echo
+
+  # delete all
+  mkdir -p etc/
+  rm -f etc/*
+
+  # .orig, .base, and .conf files exist
+  for config in ${config_names}; do
+    cp rootfs.ro/etc/${config}.orig etc/${config}.orig
+    chmod +w etc/${config}.orig
+    md5sum `realpath etc/${config}.orig` > etc/${config}.orig.md5
+    cp etc/${config}.orig etc/${config}.base
+    md5sum `realpath etc/${config}.base` > etc/${config}.base.md5
+    cp etc/${config}.orig etc/${config}.conf
+    md5sum `realpath etc/${config}.conf` > etc/${config}.conf.md5
+  done
+
+  # prior to the update, hostapd.conf and wpa_supplicant.conf had ssid changed
+
+  sed -i "s/^ssid=SoloLink_.*/ssid=SoloLink_Test/" etc/hostapd.conf
+  md5sum `realpath etc/hostapd.conf` > etc/hostapd.conf.md5
+
+  echo "network={"                  >> etc/wpa_supplicant.conf
+  echo "	ssid=\"SoloLink_Test\"" >> etc/wpa_supplicant.conf
+  echo "	key_mgmt=NONE"          >> etc/wpa_supplicant.conf
+  echo "}"                          >> etc/wpa_supplicant.conf
+  md5sum `realpath etc/wpa_supplicant.conf` > etc/wpa_supplicant.conf.md5
+
+  # telemetry baud was changed in sololink.conf
+  sed -i "s/^telemBaud=.*/telemBaud=115200/" etc/sololink.conf
+  md5sum `realpath etc/sololink.conf` > etc/sololink.conf.md5
+
+  # as part of update, we change something in hostapd.orig
+  sed -i "s/^acs_num_scans=.*/acs_num_scans=1000/" etc/hostapd.orig
+
+  # we do not change wpa_supplicant.orig
+
+  # we add a new setting to sololink.orig
+  sed -i "s/useGpsTime=True/useGpsTime=True\n\nuseOuijiTime=Definitely/" etc/sololink.orig
+
+  # .orig.md5 is no longer correct for the updated versions
+  # (that is how the update is detected)
+
+  # show changes
+  for config in ${config_names}; do
+    echo "user changes in ${config}.conf:"
+    diff etc/${config}.base etc/${config}.conf
+    echo
+    echo "update changes in ${config}.orig:"
+    diff rootfs.ro/etc/${config}.orig etc/${config}.orig
+    echo
+  done
+
+  ./configinit ${verbose}
+
+  # should have:
+  # * .base, .base.md5
+  # * .conf, .conf.md5
+  # * .orig, .orig.md5
+  # * .base and .orig should be the same, no user changes
+  # * .conf should have both update and user changes
+  for config in ${config_names}; do
+    assert_valid etc/${config}.base
+    assert_valid etc/${config}.conf
+    assert_valid etc/${config}.orig
+    assert_same etc/${config}.orig etc/${config}.base
+    echo "${config}.conf should have both user and update mods:"
+    diff rootfs.ro/etc/${config}.orig etc/${config}.conf
+  done
+
+  echo
+  echo "##### test_update complete"
+  echo
+}
+
+test_first_boot
+
+test_normal_boot
+
+test_invalid_conf
+
+test_update
+
+# clean up
+rm -rf ./etc

sololink/flightcode/Makefile

@@ -0,0 +1,51 @@
+
+SUBDIRS =
+SUBDIRS += arp_table
+SUBDIRS += dflog
+SUBDIRS += dataflash_logger
+SUBDIRS += hostapd_ctrl
+SUBDIRS += pixrc
+SUBDIRS += proc_top
+SUBDIRS += rssi
+SUBDIRS += stm32
+SUBDIRS += telem
+SUBDIRS += telem_ctrl
+SUBDIRS += tlog
+SUBDIRS += video/vid
+SUBDIRS += video/app
+SUBDIRS += video/hdmi
+SUBDIRS += unlock
+SUBDIRS += wdog
+
+SUBDIRS_BUILD = $(SUBDIRS:%=%_build)
+SUBDIRS_CLEAN = $(SUBDIRS:%=%_clean)
+
+# SUBDIRS2 is SUBDIRS, plus directories that can be formatted but not built
+SUBDIRS2 = $(SUBDIRS)
+SUBDIRS2 += ini
+SUBDIRS2 += ini/cpp
+SUBDIRS2 += log
+SUBDIRS2 += util
+
+SUBDIRS_FMT = $(SUBDIRS2:%=%_fmt)
+SUBDIRS_FMT_DIFF = $(SUBDIRS2:%=%_fmt-diff)
+
+all: $(SUBDIRS_BUILD)
+
+build: $(SUBDIRS_BUILD)
+$(SUBDIRS_BUILD):
+	$(MAKE) -C $(@:%_build=%)
+
+clean: $(SUBDIRS_CLEAN)
+$(SUBDIRS_CLEAN):
+	$(MAKE) -C $(@:%_clean=%) clean
+
+fmt: $(SUBDIRS_FMT)
+$(SUBDIRS_FMT):
+	$(MAKE) -C $(@:%_fmt=%) fmt
+
+fmt-diff: $(SUBDIRS_FMT_DIFF)
+$(SUBDIRS_FMT_DIFF):
+	$(MAKE) -C $(@:%_fmt-diff=%) fmt-diff
+
+.PHONY: $(SUBDIRS) $(SUBDIRS_BUILD) $(SUBDIRS_CLEAN) $(SUBDIRS_FMT) $(SUBDIRS_FMT_DIFF)

sololink/flightcode/arp_table/Makefile

@@ -0,0 +1,40 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+VPATH = ../util
+
+INCS = -I../util
+
+CFLAGS += -Wall $(INCS)
+
+SRCS_C = main.c arp_table.c util.c
+
+OBJS = $(SRCS_C:.c=.o)
+
+MAIN = arp_table
+
+all: $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(LINK.c) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/arp_table/main.c

@@ -0,0 +1,33 @@
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include "arp_table.h"
+
+#define MAX_ARP_ENTRIES 10
+arp_entry_t arp_table[MAX_ARP_ENTRIES];
+
+int main(int argc, char *argv[])
+{
+    int arp_entries;
+    int i;
+
+    arp_entries = MAX_ARP_ENTRIES;
+    if (arp_table_get(arp_table, &arp_entries) != 0) {
+        printf("ERROR reading arp table\n");
+        exit(1);
+    }
+
+    for (i = 0; i < arp_entries; i++) {
+        struct in_addr in;
+        in.s_addr = arp_table[i].ip;
+        uint8_t *mac = arp_table[i].mac;
+        printf("%-15s 0x%x 0x%x %02x:%02x:%02x:%02x:%02x:%02x %s\n", inet_ntoa(in),
+               arp_table[i].hw_type, arp_table[i].flags, mac[0], mac[1], mac[2], mac[3], mac[4],
+               mac[5], arp_table[i].dev);
+    }
+
+    exit(0);
+
+} /* main */

sololink/flightcode/baudcheck/Makefile

@@ -0,0 +1,37 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+VPATH = ../util
+
+INCS = -I../util
+
+CFLAGS += -Wall $(INCS) 
+CXXFLAGS += -Wall $(INCS)
+
+LIBS += -lpthread
+
+SRCS_CPP = baudcheck.cpp
+SRCS_C += util.c
+
+OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
+
+MAIN = baudcheck
+
+all: $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+	$(RM) ../util/*.o
+
+.PHONY: clean

sololink/flightcode/baudcheck/baudcheck.cpp

@@ -0,0 +1,371 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <iomanip>
+#include <stdint.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <netdb.h>
+#include <time.h>
+#include <termios.h>
+#include <sys/errno.h>
+#include <sys/ioctl.h>
+#include <list>
+#include <sys/mman.h>
+#include <fstream>
+#include <poll.h>
+#include "util.h"
+#include "../mavlink/c_library/ardupilotmega/mavlink.h"
+#include "../mavlink/c_library/common/mavlink.h"
+#include <sstream>
+#include <signal.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <dirent.h>
+
+using namespace std;
+
+/***********************************************************************
+Serial port name
+***********************************************************************/
+string serialPortName;
+
+/***********************************************************************
+File descriptors
+***********************************************************************/
+int serial_fd;
+
+/***********************************************************************
+Threading variables
+***********************************************************************/
+pthread_mutex_t mutex_msg;
+pthread_mutex_t mutex_msgtype;
+mavlink_message_t latest_msg;
+sem_t sem_msg;
+int msg_type = 0;
+
+/***********************************************************************
+Serial tx/rx buffer size
+***********************************************************************/
+#define BUFSIZE 256
+
+/***********************************************************************
+Function: int serial_setup(int baud)
+
+Description: The serial port initialization function.  This function
+             initializes the serial port over which DSM data is sent to
+             the pixhawk.  A return of 0 indicates an error.
+***********************************************************************/
+int serial_setup(int baud)
+{
+    struct termios options;
+
+    serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY);
+
+    if(serial_fd < 0)
+    {
+        cerr << "Unable to open serial port " << serialPortName.c_str() << endl;
+        return 0;
+    }
+
+    tcflush(serial_fd, TCIOFLUSH);
+
+    //Configure port for 8N1 transmission
+    tcgetattr(serial_fd, &options);      //Gets the current options for the port
+    //Set the output baud rate
+    switch(baud)
+    {
+        case 57600: cfsetspeed(&options, B57600); break;
+        case 115200: cfsetspeed(&options, B115200); break;
+        case 1500000: cfsetspeed(&options, B1500000); break;
+        default:
+            cerr << "Unsupported baud rate" << endl;
+            return 0;
+    }
+    options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+    options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
+    options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+    options.c_cflag &= ~(CSIZE | PARENB);
+    options.c_cflag |= CS8;
+    options.c_cflag |= CRTSCTS;
+    options.c_cc[VMIN]  = 17;
+    options.c_cc[VTIME] = 0;
+
+    tcsetattr(serial_fd, TCSANOW, &options);    //Set the new options for the port "NOW"
+
+    //sleep(1);
+    tcflush(serial_fd, TCIOFLUSH);
+
+    cout << "Opened serial port " << serialPortName.c_str() << endl;
+
+    return 1;
+}
+
+void connectUSB(void)
+{
+    system("echo 19 >> /sys/class/gpio/export");
+    system("echo 21 >> /sys/class/gpio/export");
+    system("echo out >> /sys/class/gpio/gpio19/direction");
+    system("echo out >> /sys/class/gpio/gpio21/direction");
+    system("echo 1 >> /sys/class/gpio/gpio21/value");
+    system("echo 0 >> /sys/class/gpio/gpio19/value");
+
+}
+
+void disconnectUSB(void)
+{
+    system("echo 0 >> /sys/class/gpio/gpio21/value");
+    system("echo 0 >> /sys/class/gpio/gpio19/value");
+    system("echo 19 >> /sys/class/gpio/unexport");
+    system("echo 21 >> /sys/class/gpio/unexport");
+}
+
+/***********************************************************************
+Function: int mavlink_task()
+
+Description: Thread task to pull mavlink data from the serial port
+             and dump it into a mutex-protected mavlink message.
+
+             When log data begins streaming, dumps log data into the 
+             log file on the filesystem.
+***********************************************************************/
+void *mavlink_task(void*)
+{
+    mavlink_message_t _msg;
+    mavlink_status_t mavlink_status;
+    int read_len;
+    int i;
+    char buf[BUFSIZE];
+    int msgtolookfor;
+
+    while(1)
+    {
+        read_len = read(serial_fd, buf, BUFSIZE);
+
+        if (read_len < 0)
+        {
+            if (errno != EAGAIN)
+                cerr << "Read err: " << errno << endl;
+        }
+
+        if(read_len > 0)
+        {
+            for(i=0; i<read_len; ++i)
+            {
+                if(mavlink_parse_char(MAVLINK_COMM_0, buf[i], &_msg, &mavlink_status))
+                {
+                    //If this is the type of message we're waiting for, put it on
+                    //the latest_msg or handle its log data.
+                    pthread_mutex_lock(&mutex_msgtype);
+                    msgtolookfor = msg_type;
+                    pthread_mutex_unlock(&mutex_msgtype);
+
+                    if(_msg.msgid == msgtolookfor)
+                    {
+                        pthread_mutex_lock(&mutex_msg);
+                        memcpy(&latest_msg, &_msg, sizeof(mavlink_message_t));
+                        pthread_mutex_unlock(&mutex_msg);
+                        sem_post(&sem_msg);
+                    }
+                }
+            }
+        }
+    }
+    pthread_exit(NULL);
+}
+
+/***********************************************************************
+Function: int wait_for_message()
+
+Description: Blocks until a particular mavlink message type is received.
+             A return value of 0 indicates succes, -1 for a timeout.
+             Setting timeout_s to 0 blocks indefinitely.
+***********************************************************************/
+int wait_for_message(int _msg_type, mavlink_message_t *msg, int timeout_s)    
+{
+    struct timespec timeout;
+    int ret;
+
+    //Tell the mavlink thread which message to look for
+    pthread_mutex_lock(&mutex_msgtype);
+    msg_type = _msg_type;
+    pthread_mutex_unlock(&mutex_msgtype);
+
+    clock_gettime(CLOCK_REALTIME, &timeout);
+    timeout.tv_sec += timeout_s;
+    
+    if(timeout_s == 0)
+        sem_wait(&sem_msg);
+    else
+    {
+        ret = sem_timedwait(&sem_msg, &timeout);
+
+        if(ret < 0)
+        {
+            if (errno == ETIMEDOUT)
+                return -1;
+        }
+    }
+
+    pthread_mutex_lock(&mutex_msg);
+    memcpy(msg, &latest_msg, sizeof(mavlink_message_t));
+    pthread_mutex_unlock(&mutex_msg);
+
+    return 0;
+}
+
+int open_USB_serial(void)
+{
+    char usbdev_string[256] = "usb-3D_Robotics_PX4_FMU";
+    struct dirent *direntry;
+    bool foundDevice = false;
+    DIR *dir;
+
+    connectUSB();
+    sleep(1);
+
+    dir = opendir("/dev/serial/by-id/");
+    if (dir == NULL) {
+        cerr << "open /dev/serial/by-id failed" << endl;
+        return -1;
+    }
+
+    while((direntry = readdir(dir)))
+    {
+        if(!strncmp(direntry->d_name, usbdev_string, 23))
+        {
+            foundDevice = true;
+            strcpy(usbdev_string,direntry->d_name);
+            break;
+        }
+    }
+
+    if(!foundDevice)
+    {
+        cerr << "Unable to find USB device" << endl;
+        return -1;
+    }
+
+    closedir(dir);
+
+    //Serial port setup
+    serialPortName = "/dev/serial/by-id/";
+    serialPortName.append(usbdev_string);
+    if(!serial_setup(115200))
+    {
+        cerr << "Unable to initialize the USB serial port" << endl;
+        return -1;
+    }
+
+    return 0;
+}
+
+/***********************************************************************
+Function: void request_baudrate()
+
+Description: Request a particular log file from the pixhawk.
+***********************************************************************/
+void request_baudrate(void)
+{
+    char buf[BUFSIZE];
+    mavlink_message_t msg;
+    int msg_len;
+    float baud;
+    int tries;
+    bool gotParam=false;
+        
+    mavlink_msg_param_request_read_pack(1, 1, &msg, 1, 1, "SERIAL1_BAUD", -1);
+    msg_len = mavlink_msg_to_send_buffer((uint8_t*)buf,&msg);
+
+    if(write(serial_fd, buf, msg_len) != msg_len)
+    {
+        cerr << "Serial port write error." << endl;
+        disconnectUSB();
+        exit(1);
+    }
+
+    for(tries=0; tries<3; ++tries)
+    {
+        cout << "Requesting SERIAL1_BAUD..." << endl;
+
+        if(wait_for_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, 3) >= 0)
+        {
+            baud = mavlink_msg_param_value_get_param_value(&msg);
+            cout << "Got param value " << baud << endl;
+            disconnectUSB();
+            exit(0);
+        }
+    }
+
+    if(!gotParam)
+    {
+        cout << "Unable to get SERIAL1_BAUD_VALUE" << endl;
+        disconnectUSB();
+        exit(1);
+    }
+}
+
+/**********************************************************************
+Function: int main(void)
+
+Description: The main function.  Initializes and runs the serial and
+             UDP threads.
+***********************************************************************/
+int main(int argc, char *argv[])
+{
+    int baudrate;
+    pthread_t mavlink_ctx;
+    mavlink_message_t msg;
+
+    //Cout cleanup
+    std::cout.precision(2);
+    std::cout.setf( ios::fixed, ios::floatfield );
+
+    cout << "Pixhawk baudrate checker" << endl;
+    
+    //Serial port setup
+    serialPortName = "/dev/ttymxc1";
+    baudrate=115200;//57600;
+    if(!serial_setup(baudrate))
+    {
+        cerr << "Unable to initialize the serial send" << endl;
+        return -1;
+    }
+
+    //Threading initialization
+    pthread_mutex_init(&mutex_msg, NULL);
+    sem_init(&sem_msg, 0, 0);
+    
+    //Start the mavlink rx thread
+    pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL);
+
+    cout << "Waiting for a hearbeat..." << endl;
+    
+    //Wait for a heartbeat on the telem port
+    if(wait_for_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 5) < 0)
+    {
+        cout << "No heartbeat received, requesting baudrate from USB" << endl;
+        pthread_cancel(mavlink_ctx);
+        close(serial_fd);
+        if (open_USB_serial() < 0)
+            return -1;
+    }
+    else
+    {
+        cout << "Got a heartbeat, exiting" << endl;
+        return 0;
+    }
+    
+    pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL);
+
+    //Send a param request to the pixhawk
+    request_baudrate();
+
+    //Wait for the mavlink thread to end
+    pthread_join(mavlink_ctx, NULL);
+
+    return 0;
+}

sololink/flightcode/command/Commander.cpp

@@ -0,0 +1,370 @@
+
+#include <syslog.h>
+#include <errno.h>
+#include <pthread.h>
+#include <unistd.h>
+#include <stddef.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <sys/un.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <iostream>
+#include "util.h"
+#include "mutex.h"
+#include "Commander.h"
+
+using namespace std;
+
+
+// Constructor creates and opens the command socket, then starts the command
+// thread.
+Commander::Commander(const char *sock_name)
+{
+
+    if (strlen(sock_name) >= sizeof(sa.sun_path)) {
+        cerr << "ERROR: command socket name too long" << endl;
+        return;
+    }
+
+    // If there is something already there with the socket name, it is deleted.
+    // If not, unlink() fails (and we don't care).
+    (void)unlink(sock_name);
+
+    // create command socket
+    _fd = socket(AF_UNIX, SOCK_DGRAM, 0);
+    if (_fd < 0) {
+        cerr << "ERROR creating command socket" << endl;
+        return;
+    }
+
+    // bind command socket to address
+    memset(&sa, 0, sizeof(sa));
+    sa.sun_family = AF_UNIX;
+    strncpy(sa.sun_path, sock_name, sizeof(sa.sun_path) - 1);
+    if (bind(_fd, (struct sockaddr *)&sa, sizeof(sa)) != 0) {
+        cerr << "ERROR binding command socket" << endl;
+        close(_fd);
+        _fd = -1;
+        return;
+    }
+
+    // initialize clients list mutex
+    if (mutex_init(&clients_mutex) != 0) {
+        cerr << "ERROR initializing clients mutex" << endl;
+        close(_fd);
+        _fd = -1;
+        return;
+    }
+
+    // start command thread
+    if (pthread_create(&thread_id, NULL, &Commander::command_entry, this) != 0) {
+        cerr << "ERROR starting command thread" << endl;
+        close(_fd);
+        _fd = -1;
+        return;
+    }
+
+} // Commander::Commander
+
+
+// Destructor sends a QUIT command to the command thread, waits for it to exit,
+// then closes the command socket.
+Commander::~Commander()
+{
+    const char *quit = "QUIT";
+    ssize_t quit_len = strlen(quit);
+
+    if (sendto(_fd, quit, quit_len, 0,
+               (const struct sockaddr *)&sa, sizeof(sa)) != quit_len) {
+        cerr << "~Commander: ERROR returned from sendto" << endl;
+    }
+
+    if (pthread_join(thread_id, NULL) != 0) {
+        cerr << "ERROR returned from pthread_join" << endl;
+    }
+
+    close(_fd);
+
+} // Commander::~Commander()
+
+
+// Commander processing thread.
+// Wait for a command on the command socket, and process each. A QUIT command
+// causes the loop to exit.
+void *Commander::command_thread(void)
+{
+    bool quit = false;
+    char buf[MAX_CMD_LEN];
+    int nb;
+    struct sockaddr_storage src_storage;
+    struct sockaddr *src = (struct sockaddr *)&src_storage;
+    socklen_t src_len;
+    char *token;
+    const char *delims;
+
+    //cout << "command_thread: running" << endl;
+
+    while (!quit) {
+
+        src_len = sizeof(struct sockaddr_storage);
+        memset(src, 0, src_len);
+        memset(&buf, 0, sizeof(buf));
+        nb = recvfrom(_fd, buf, MAX_CMD_LEN, 0, src, &src_len);
+        if (nb < 0) {
+            cerr << "ERROR returned from recvfrom" << endl;
+            continue;
+        }
+
+        delims = " \t\r\n";
+        token = strtok(buf, delims);
+
+        if (token == NULL) {
+            // no command, like ping
+            handle_ping(src, src_len);
+        } else if (strcasecmp(token, "ATTACH") == 0) {
+            token = strtok(NULL, delims);
+            handle_attach(src, src_len, token, true);
+        } else if (strcasecmp(token, "DETACH") == 0) {
+            token = strtok(NULL, delims);
+            handle_attach(src, src_len, token, false);
+        } else if (strcasecmp(token, "PING") == 0) {
+            handle_ping(src, src_len);
+        } else if (strcasecmp(token, "LIST") == 0) {
+            handle_list(src, src_len);
+        } else if (strcasecmp(token, "QUIT") == 0) {
+            quit = true;
+        } else {
+            cerr << "Unknown command: " << buf << endl;
+        }
+
+    } // while (!quit)
+
+    //cout << "command_thread: exiting" << endl;
+
+    return NULL;
+
+} // Commander::command_thread
+
+
+// Send data to all clients.
+// The supplied data is sent to all clients in the client list, i.e. all
+// clients for which an ATTACH command has been sent. Unix domain datagram
+// sockets are reliable, which means a write will block if the reader is
+// not fast enough. We can't let an RC packet consumer block the UDP thread,
+// so the sendto is done nonblocking. This means it is up to the reader to
+// read fast enough to keep from dropping packets.
+void Commander::send_clients(const void *data, int data_len)
+{
+    static unsigned drops = 0;
+    vector<SockAddr>::iterator it;
+
+    pthread_mutex_lock(&clients_mutex);
+
+    for (it = clients.begin(); it != clients.end(); it++) {
+        if (sendto(_fd, data, data_len, MSG_DONTWAIT,
+                   it->sockaddr(), it->socklen()) != data_len) {
+            //cerr << "send_clients: \"" << strerror(errno)
+            //     << "\" sending to " << *it << endl;
+            drops++;
+        } else {
+            //cout << "send_clients: sent to " << *it << endl;
+            if (drops > 0) {
+                syslog(LOG_INFO, "cmd: dropped %d packets", drops);
+                drops = 0;
+            }
+        }
+    }
+
+    pthread_mutex_unlock(&clients_mutex);
+
+} // Commander::send_clients
+
+
+// Handle ATTACH or DETACH command.
+//
+// ATTACH/DETACH takes and optional argument indicating the client to ATTACH or
+// detach. If no argument is given, the sender of the command is used. If an
+// argument is given, it should be an AF_UNIX socket (support for UDP sockets
+// could be added).
+//
+// If the command was ATTACH and the client is not in the list of clients, add
+// it to the list. If the command was DETACH and the client is in the list of
+// clients, remove it from the list.
+void Commander::handle_attach(const struct sockaddr *src_addr,
+                              socklen_t src_addr_len, const char *client,
+                              bool attach)
+{
+    struct sockaddr_storage client_addr;
+    socklen_t client_addr_len;
+
+    memset(&client_addr, 0, sizeof(client_addr));
+    client_addr_len = 0;
+
+    if (client == NULL) {
+        // no client given - use source of request as client
+        memcpy(&client_addr, src_addr, src_addr_len);
+        client_addr_len = src_addr_len;
+    } else {
+        // client should be a unix socket name
+        // (support for IP:port could be added)
+        struct stat stat_buf;
+        if (stat(client, &stat_buf) != 0) {
+            cerr << "handle_attach: ERROR: \"" << client
+                 << "\" does not exist" << endl;
+            return;
+        }
+        if (!S_ISSOCK(stat_buf.st_mode)) {
+            cerr << "handle_attach: ERROR: \"" << client
+                 << "\" exists but is not a socket" << endl;
+            return;
+        }
+        struct sockaddr_un *c_un = (struct sockaddr_un *)&client_addr;
+        int maxlen = sizeof(c_un->sun_path);
+        c_un->sun_family = AF_UNIX;
+        strncpy(c_un->sun_path, client, maxlen - 1);
+        c_un->sun_path[maxlen - 1] = '\0';
+        client_addr_len = offsetof(struct sockaddr_un, sun_path)
+                        + strlen(c_un->sun_path) + 1; // man 7 unix
+    }
+
+    SockAddr s((struct sockaddr *)&client_addr, client_addr_len);
+    bool found = false;
+    vector<SockAddr>::iterator it;
+
+    pthread_mutex_lock(&clients_mutex);
+
+    for (it = clients.begin(); it != clients.end(); it++) {
+        if (*it == s) { // == overloaded
+            found = true;
+            break;
+        }
+    }
+
+    if (attach && !found) {
+        // attach new client
+        clients.push_back(s);
+    } else if (!attach && found) {
+        // detach existing client
+        clients.erase(it);
+    }
+
+    pthread_mutex_unlock(&clients_mutex);
+
+    send_response(src_addr, src_addr_len, "OK\n");
+
+} // Commander::handle_attach
+
+
+// Handle LIST command.
+// Response to LIST is attached clients, one per line, then "OK\n".
+void Commander::handle_list(const struct sockaddr *src_addr,
+                          socklen_t src_addr_len)
+{
+    string response;
+
+    pthread_mutex_lock(&clients_mutex);
+
+    vector<SockAddr>::iterator it;
+    for (it = clients.begin(); it != clients.end(); it++)
+        response = response + it->to_string() + "\n";
+
+    pthread_mutex_unlock(&clients_mutex);
+
+    response += "OK\n";
+
+    send_response(src_addr, src_addr_len, response.c_str());
+
+} // Commander::handle_list
+
+
+// Handle PING command.
+// Response to PING is simply "OK\n".
+void Commander::handle_ping(const struct sockaddr *src_addr,
+                          socklen_t src_addr_len)
+{
+    send_response(src_addr, src_addr_len, "OK\n");
+}
+
+
+// Send a reponse to a client command.
+void Commander::send_response(const struct sockaddr *src_addr,
+                            socklen_t src_addr_len,
+                            const char *response, ssize_t response_len)
+{
+    if (response_len == -1)
+        response_len = strlen(response);
+
+    if (sendto(_fd, response, response_len, 0, src_addr, src_addr_len)
+        != response_len) {
+        cerr << "send_response: ERROR returned from sendto" << endl;
+            cerr << "send_response: \"" << strerror(errno)
+                 << "\" sending to " << *src_addr << endl;
+    } else {
+        //cout << "send_response: OK" << endl;
+    }
+
+} // Commander::send_response
+
+
+// print SockAddr to stream (human-readable)
+ostream& operator<<(ostream& os, const SockAddr& sock)
+{
+    sa_family_t family = sock.su.sa.sa_family;
+
+    if (family == AF_UNIX) {
+        const struct sockaddr_un *un = &sock.su.un;
+        os << "AF_UNIX: " << un->sun_path;
+    } else if (family == AF_INET) {
+        const struct sockaddr_in *in = &sock.su.in;
+        os << "AF_INET: " << inet_ntoa(in->sin_addr) << ":"
+            << ntohs(in->sin_port);
+    } else {
+        os << "sa_family=" << family;
+    }
+    return os;
+}
+
+
+// print sockaddr/sockaddr_un/sockaddr_in to stream (human-readable)
+ostream& operator<<(ostream& os, const struct sockaddr& sa)
+{
+    sa_family_t family = sa.sa_family;
+
+    if (family == AF_UNIX) {
+        const struct sockaddr_un *un = (const struct sockaddr_un *)&sa;
+        os << "AF_UNIX: " << un->sun_path;
+    } else if (family == AF_INET) {
+        const struct sockaddr_in *in = (const struct sockaddr_in *)&sa;
+        os << "AF_INET: " << inet_ntoa(in->sin_addr) << ":"
+            << ntohs(in->sin_port);
+    } else {
+        os << "sa_family=" << family;
+    }
+    return os;
+}
+
+
+#if 0
+
+int main(int argc, char *argv[])
+{
+    Commander *ss;
+
+    cout << "main: creating command object" << endl;
+    ss = new Commander(argv[1]);
+
+    for (int i = 0; i < 4; i++) {
+        ss->send_clients("hello\n", 6);
+        sleep(5);
+    }
+
+    cout << "main: deleting command object" << endl;
+    delete ss;
+
+    cout << "main: exiting" << endl;
+
+} // main
+
+#endif 

sololink/flightcode/command/Commander.h

@@ -0,0 +1,128 @@
+
+#include <pthread.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <vector>
+#include <string>
+#include <iostream>
+#include <sstream>
+
+#include "Log.h"
+
+using namespace std;
+
+
+ostream& operator<<(ostream& os, const struct sockaddr& sa);
+
+
+// Simple wrapper for sockaddr and its variants
+class SockAddr
+{
+
+    public:
+
+        // Constructor mainly just checks that the supplied sockaddr fits
+        // (else is is probably corrupt).
+        SockAddr(struct sockaddr *s, socklen_t len)
+        {
+            memset(&su, 0, sizeof(su));
+            sl = len;
+            if (sl > sizeof(su))
+                sl = sizeof(su);
+            memcpy(&su, s, sl);
+        }
+
+        // Destructor doesn't need to do anything; memory cleared for fun.
+        ~SockAddr()
+        {
+            memset(&su, 0, sizeof(su));
+            sl = 0;
+        }
+
+        // Used when adding/deleting a new SockAddr to see if a particular
+        // one is already in the list
+        const bool operator==(const SockAddr& rhs)
+        {
+            // address does not have to match after 'sl' bytes
+            return (sl == rhs.sl) && (memcmp(&su, &rhs.su, sl) == 0);
+        }
+
+        // 'string' representation is used in the response to a 'list'
+        // command, where a list of all SockAddrs is being sent back. Uses
+        // operator<< for the conversion.
+        string to_string(void)
+        {
+            ostringstream ss;
+            ss << *this;
+            return ss.str();
+        }
+
+        // member accessor
+        const struct sockaddr *sockaddr()
+        {
+            return &su.sa;
+        }
+
+        // member accessor
+        const socklen_t socklen()
+        {
+            return sl;
+        }
+
+        friend ostream& operator<<(ostream&, const SockAddr&);
+
+    private:
+
+        // The actual socket address. In most cases an anonymous union would
+        // be better, but there are some cases where we want the maximum size.
+        union {
+            struct sockaddr sa;
+            struct sockaddr_in in;
+            struct sockaddr_un un;
+        } su;
+
+        // sockaddr_un requires the length to fully specify the address
+        // (see 'man 7 unix', pathname vs. unnamed vs. abstract). We only
+        // support the pathname variant a the moment, but this is maintained
+        // for possible future enhancement.
+        socklen_t sl;
+
+}; // SockAddr
+
+
+class Commander
+{
+    public:
+        Commander(const char *sock_name);
+        ~Commander();
+        void send_clients(const void *data, int num_bytes);
+
+    protected:
+        int _fd;
+        virtual void handle_attach(const struct sockaddr *, socklen_t,
+                                   const char *, bool);
+        virtual void handle_list(const struct sockaddr *, socklen_t);
+        virtual void handle_ping(const struct sockaddr *, socklen_t);
+        void send_response(const struct sockaddr *, socklen_t,
+                           const char *, ssize_t response_len=-1);
+
+    private:
+        struct sockaddr_un sa;
+        pthread_t thread_id;
+        vector<SockAddr> clients;
+        pthread_mutex_t clients_mutex;
+
+        void *command_thread(void);
+
+        static const int MAX_CMD_LEN = 64;
+
+        // the purpose of this is to give pthread_create a class static
+        // function that calls back to an object method
+        static void *command_entry(void *arg)
+        {
+            Commander *me = (Commander *)arg;
+            return me->command_thread();
+        }
+};

sololink/flightcode/command/Makefile

@@ -0,0 +1,38 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+CFLAGS += -Wall -g
+CXXFLAGS += -Wall -g
+
+LIBS = -lpthread
+
+SRCS_CPP = Commander.cpp
+#SRCS_CPP += ../ini/cpp/INIReader.cpp ../log/Log.cpp
+#SRCS_C = ../ini/ini.c
+
+OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
+
+MAIN = Commander
+
+all: $(MAIN)
+
+Commander.o: Commander.cpp Commander.h
+
+$(MAIN): $(OBJS)
+	$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+	#$(RM) ../ini/*.o
+	#$(RM) ../ini/cpp/*.o
+	#$(RM) ../log/*.o
+
+.PHONY: clean

sololink/flightcode/dataflash_logger/Makefile

@@ -0,0 +1,56 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+VPATH = ../util ../ini ../ini/cpp
+
+INCS = -I../util -I../ini -I../ini/cpp
+
+CFLAGS += -Wall $(INCS)
+CXXFLAGS += -Wall $(INCS) -std=c++11
+
+SRCS_CPP = dataflash_logger.cpp
+SRCS_CPP += common_tool.cpp
+SRCS_CPP += format_reader.cpp
+SRCS_CPP += telem_forwarder_client.cpp
+SRCS_CPP += mavlink_reader.cpp
+SRCS_CPP += analyzer_util.cpp
+SRCS_CPP += dataflash_logger_program.cpp
+SRCS_CPP += INIReader.cpp
+SRCS_CPP += mavlink_message_handler.cpp
+SRCS_CPP += mavlink_writer.cpp
+SRCS_CPP += telem_client.cpp
+SRCS_CPP += telem_serial.cpp
+SRCS_CPP += la-log.cpp
+SRCS_CPP += heart.cpp
+SRCS_C = util.c ini.c
+
+OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
+LIBS=-pthread
+
+MAIN = dataflash_logger # actually, the main is in mavlink_reader...
+
+all: $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: clean

sololink/flightcode/dataflash_logger/MsgHandler.cpp

@@ -0,0 +1,264 @@
+#include "MsgHandler.h"
+
+#include <string.h>
+
+void fatal(const char *msg)
+{
+    ::printf("%s", msg);
+    ::printf("\n");
+    exit(1);
+}
+
+char *xstrdup(const char *string)
+{
+    char *ret = strdup(string);
+    if (ret == NULL) {
+        perror("strdup");
+        fatal("strdup failed");
+    }
+    return ret;
+}
+
+void MsgHandler::add_field_type(char type, size_t size)
+{
+    size_for_type_table[(type > 'A' ? (type - 'A') : (type - 'a'))] = size;
+}
+
+uint8_t MsgHandler::size_for_type(char type)
+{
+    return size_for_type_table[(uint8_t)(type > 'A' ? (type - 'A') : (type - 'a'))];
+}
+
+void MsgHandler::init_field_types()
+{
+    add_field_type('b', sizeof(int8_t));
+    add_field_type('c', sizeof(int16_t));
+    add_field_type('e', sizeof(int32_t));
+    add_field_type('f', sizeof(float));
+    add_field_type('h', sizeof(int16_t));
+    add_field_type('i', sizeof(int32_t));
+    add_field_type('n', sizeof(char[4]));
+    add_field_type('B', sizeof(uint8_t));
+    add_field_type('C', sizeof(uint16_t));
+    add_field_type('E', sizeof(uint32_t));
+    add_field_type('H', sizeof(uint16_t));
+    add_field_type('I', sizeof(uint32_t));
+    add_field_type('L', sizeof(int32_t));
+    add_field_type('M', sizeof(uint8_t));
+    add_field_type('N', sizeof(char[16]));
+    add_field_type('Z', sizeof(char[64]));
+    add_field_type('q', sizeof(int64_t));
+    add_field_type('Q', sizeof(uint64_t));
+}
+
+struct MsgHandler::format_field_info *MsgHandler::find_field_info(const char *label)
+{
+    for (uint8_t i = 0; i < next_field; i++) {
+        if (streq(field_info[i].label, label)) {
+            return &field_info[i];
+        }
+    }
+    return NULL;
+}
+
+MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f)
+{
+    init_field_types();
+    parse_format_fields();
+}
+
+void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset, uint8_t _length)
+{
+    field_info[next_field].label = xstrdup(_label);
+    field_info[next_field].type = _type;
+    field_info[next_field].offset = _offset;
+    field_info[next_field].length = _length;
+    next_field++;
+}
+
+void MsgHandler::parse_format_fields()
+{
+    char *labels = xstrdup(f.labels);
+    char *arg = labels;
+    uint8_t label_offset = 0;
+    char *next_label;
+    uint8_t msg_offset = 3; // 3 bytes for the header
+
+    while ((next_label = strtok(arg, ",")) != NULL) {
+        if (label_offset > strlen(f.format)) {
+            free(labels);
+            printf("too few field times for labels %s (format=%s) (labels=%s)\n", f.name, f.format,
+                   f.labels);
+            exit(1);
+        }
+        uint8_t field_type = f.format[label_offset];
+        uint8_t length = size_for_type(field_type);
+        add_field(next_label, field_type, msg_offset, length);
+        arg = NULL;
+        msg_offset += length;
+        label_offset++;
+    }
+
+    if (label_offset != strlen(f.format)) {
+        printf("too few labels for format (format=%s) (labels=%s)\n", f.format, f.labels);
+    }
+
+    free(labels);
+}
+
+bool MsgHandler::field_value(const uint8_t *msg, const char *label, char *ret, uint8_t retlen)
+{
+    struct format_field_info *info = find_field_info(label);
+    if (info == NULL) {
+        ::printf("No info for (%s)\n", label);
+        abort();
+    }
+
+    uint8_t offset = info->offset;
+    if (offset == 0) {
+        return false;
+    }
+
+    memset(ret, '\0', retlen);
+
+    memcpy(ret, &msg[offset], (retlen < info->length) ? retlen : info->length);
+
+    return true;
+}
+
+bool MsgHandler::field_value(uint8_t *msg, const char *label, Vector3f &ret)
+{
+    const char *axes = "XYZ";
+    uint8_t i;
+    for (i = 0; i < next_field; i++) {
+        if (!strncmp(field_info[i].label, label, strlen(label)) &&
+            strlen(field_info[i].label) == strlen(label) + 1) {
+            for (uint8_t j = 0; j < 3; j++) {
+                if (field_info[i].label[strlen(label)] == axes[j]) {
+                    field_value_for_type_at_offset(msg, field_info[i].type, field_info[i].offset,
+                                                   ret[j]);
+                    break; // break from finding-label loop
+                }
+            }
+        }
+        if (i == next_field) {
+            return 0; // not found
+        }
+    }
+
+    return true;
+}
+
+void MsgHandler::string_for_labels(char *buffer, uint bufferlen)
+{
+    memset(buffer, '\0', bufferlen);
+    bufferlen--;
+
+    char *pos = buffer;
+    for (uint8_t k = 0; k < LOGREADER_MAX_FIELDS; k++) {
+        if (field_info[k].label != NULL) {
+            uint8_t remaining = bufferlen - (pos - buffer);
+            uint8_t label_length = strlen(field_info[k].label);
+            uint8_t required = label_length;
+            if (pos != buffer) { // room for a comma
+                required++;
+            }
+            if (required + 1 > remaining) { // null termination
+                break;
+            }
+
+            if (pos != buffer) {
+                *pos++ = ',';
+            }
+
+            memcpy(pos, field_info[k].label, label_length);
+            pos += label_length;
+        }
+    }
+}
+
+MsgHandler::~MsgHandler()
+{
+    for (uint8_t k = 0; k < LOGREADER_MAX_FIELDS; k++) {
+        if (field_info[k].label != NULL) {
+            free(field_info[k].label);
+        }
+    }
+}
+
+void MsgHandler::location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
+                                   const char *label_long, const char *label_alt)
+{
+    loc.lat = require_field_int32_t(msg, label_lat);
+    loc.lng = require_field_int32_t(msg, label_long);
+    loc.alt = require_field_int32_t(msg, label_alt);
+    loc.options = 0;
+}
+
+void MsgHandler::ground_vel_from_msg(uint8_t *msg, Vector3f &vel, const char *label_speed,
+                                     const char *label_course, const char *label_vz)
+{
+    uint32_t ground_speed;
+    int32_t ground_course;
+    require_field(msg, label_speed, ground_speed);
+    require_field(msg, label_course, ground_course);
+    vel[0] = ground_speed * 0.01f * cosf(radians(ground_course * 0.01f));
+    vel[1] = ground_speed * 0.01f * sinf(radians(ground_course * 0.01f));
+    vel[2] = require_field_float(msg, label_vz);
+}
+
+void MsgHandler::attitude_from_msg(uint8_t *msg, Vector3f &att, const char *label_roll,
+                                   const char *label_pitch, const char *label_yaw)
+{
+    att[0] = require_field_int16_t(msg, label_roll) * 0.01f;
+    att[1] = require_field_int16_t(msg, label_pitch) * 0.01f;
+    att[2] = require_field_uint16_t(msg, label_yaw) * 0.01f;
+}
+
+void MsgHandler::field_not_found(const uint8_t *msg, const char *label)
+{
+    char all_labels[256];
+    uint8_t type = msg[2];
+    string_for_labels(all_labels, 256);
+    ::printf("Field (%s) not found for id=%d; options are (%s)\n", label, type, all_labels);
+    abort();
+}
+
+void MsgHandler::require_field(const uint8_t *msg, const char *label, char *buffer,
+                               uint8_t bufferlen)
+{
+    if (!field_value(msg, label, buffer, bufferlen)) {
+        field_not_found(msg, label);
+    }
+}
+
+float MsgHandler::require_field_float(const uint8_t *msg, const char *label)
+{
+    float ret;
+    require_field(msg, label, ret);
+    return ret;
+}
+uint8_t MsgHandler::require_field_uint8_t(const uint8_t *msg, const char *label)
+{
+    uint8_t ret;
+    require_field(msg, label, ret);
+    return ret;
+}
+int32_t MsgHandler::require_field_int32_t(const uint8_t *msg, const char *label)
+{
+    int32_t ret;
+    require_field(msg, label, ret);
+    return ret;
+}
+uint16_t MsgHandler::require_field_uint16_t(const uint8_t *msg, const char *label)
+{
+    uint16_t ret;
+    require_field(msg, label, ret);
+    return ret;
+}
+int16_t MsgHandler::require_field_int16_t(const uint8_t *msg, const char *label)
+{
+    int16_t ret;
+    require_field(msg, label, ret);
+    return ret;
+}

sololink/flightcode/dataflash_logger/MsgHandler.h

@@ -0,0 +1,152 @@
+#ifndef AP_MSGHANDLER_H
+#define AP_MSGHANDLER_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h> // for abort()
+
+#include <math.h>
+
+#define radians(x) (x / 180 * M_PI)
+#include "Vector3f.h"
+#include "Location.h"
+#include "DataFlash/LogMessage.h"
+
+#define LOGREADER_MAX_FIELDS 30
+
+#ifndef streq
+#define streq(x, y) (!strcmp(x, y))
+#endif
+
+class MsgHandler
+{
+public:
+    // constructor - create a parser for a MavLink message format
+    MsgHandler(const struct log_Format &f);
+
+    // retrieve a comma-separated list of all labels
+    void string_for_labels(char *buffer, uint bufferlen);
+
+    // field_value - retrieve the value of a field from the supplied message
+    // these return false if the field was not found
+    template < typename R >
+    bool field_value(const uint8_t *msg, const char *label, R &ret);
+
+    bool field_value(uint8_t *msg, const char *label, Vector3f &ret);
+    bool field_value(const uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen);
+
+    template < typename R >
+    void require_field(const uint8_t *msg, const char *label, R &ret)
+    {
+        if (!field_value(msg, label, ret)) {
+            field_not_found(msg, label);
+        }
+    }
+    void require_field(const uint8_t *msg, const char *label, char *buffer, uint8_t bufferlen);
+    float require_field_float(const uint8_t *msg, const char *label);
+    uint8_t require_field_uint8_t(const uint8_t *msg, const char *label);
+    int32_t require_field_int32_t(const uint8_t *msg, const char *label);
+    uint16_t require_field_uint16_t(const uint8_t *msg, const char *label);
+    int16_t require_field_int16_t(const uint8_t *msg, const char *label);
+
+private:
+    void add_field(const char *_label, uint8_t _type, uint8_t _offset, uint8_t length);
+
+    template < typename R >
+    void field_value_for_type_at_offset(const uint8_t *msg, uint8_t type, uint8_t offset, R &ret);
+
+    struct format_field_info { // parsed field information
+        char *label;
+        uint8_t type;
+        uint8_t offset;
+        uint8_t length;
+    };
+    struct format_field_info field_info[LOGREADER_MAX_FIELDS] = {};
+
+    uint8_t next_field;
+    size_t size_for_type_table[52]; // maps field type (e.g. 'f') to e.g 4 bytes
+
+    void parse_format_fields();
+    void init_field_types();
+    void add_field_type(char type, size_t size);
+    uint8_t size_for_type(char type);
+
+protected:
+    struct format_field_info *find_field_info(const char *label);
+
+    struct log_Format f; // the format we are a parser for
+    ~MsgHandler();
+
+    void location_from_msg(uint8_t *msg, Location &loc, const char *label_lat,
+                           const char *label_long, const char *label_alt);
+
+    void ground_vel_from_msg(uint8_t *msg, Vector3f &vel, const char *label_speed,
+                             const char *label_course, const char *label_vz);
+
+    void attitude_from_msg(uint8_t *msg, Vector3f &att, const char *label_roll,
+                           const char *label_pitch, const char *label_yaw);
+    void field_not_found(const uint8_t *msg, const char *label);
+};
+
+template < typename R >
+bool MsgHandler::field_value(const uint8_t *msg, const char *label, R &ret)
+{
+    struct format_field_info *info = find_field_info(label);
+    if (info == NULL) {
+        return false;
+    }
+
+    uint8_t offset = info->offset;
+    if (offset == 0) {
+        return false;
+    }
+
+    field_value_for_type_at_offset(msg, info->type, offset, ret);
+
+    return true;
+}
+
+template < typename R >
+inline void MsgHandler::field_value_for_type_at_offset(const uint8_t *msg, uint8_t type,
+                                                       uint8_t offset, R &ret)
+{
+    /* we register the types - add_field_type - so can we do without
+     * this switch statement somehow? */
+    switch (type) {
+    case 'B':
+        ret = (R)(((uint8_t *)&msg[offset])[0]);
+        break;
+    case 'c':
+    case 'h':
+        ret = (R)(((int16_t *)&msg[offset])[0]);
+        break;
+    case 'H':
+        ret = (R)(((uint16_t *)&msg[offset])[0]);
+        break;
+    case 'C':
+        ret = (R)(((uint16_t *)&msg[offset])[0]);
+        break;
+    case 'f':
+        ret = (R)(((float *)&msg[offset])[0]);
+        break;
+    case 'I':
+    case 'E':
+        ret = (R)(((uint32_t *)&msg[offset])[0]);
+        break;
+    case 'L':
+    case 'e':
+        ret = (R)(((int32_t *)&msg[offset])[0]);
+        break;
+    case 'q':
+        ret = (R)(((int64_t *)&msg[offset])[0]);
+        break;
+    case 'Q':
+        ret = (R)(((uint64_t *)&msg[offset])[0]);
+        break;
+    default:
+        ::printf("Unhandled format type (%c)\n", type);
+        ::abort();
+    }
+}
+
+#endif

sololink/flightcode/dataflash_logger/analyzer_util.cpp

@@ -0,0 +1,76 @@
+#include "analyzer_util.h"
+#include <sys/time.h>
+
+#include <math.h>
+
+void format_timestamp(char *buf, const uint8_t buflen, const uint64_t T)
+{
+    struct tm *tmp;
+    time_t t = T / 1000000;
+    tmp = localtime(&t);
+    ::strftime(buf, buflen, "%Y%m%d%H%M%S", tmp);
+}
+
+uint64_t now()
+{
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    return tv.tv_sec * (uint64_t)1000000 + tv.tv_usec;
+}
+
+double vec_len(double vec[3])
+{
+    return sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
+}
+
+double earthradius() // in metres
+{
+    return 6371000.0f;
+}
+
+double wrap_valid_longitude(const double longitude)
+{
+    if (longitude < 180.0f) {
+        return longitude;
+    }
+
+    return 180 - (longitude - 180);
+    // return (((longitude + 180.0) % 360.0) -180.0);
+}
+
+// http://www.movable-type.co.uk/scripts/latlong.html
+void gps_newpos(const double orig_lat, const double orig_lon, const double bearing,
+                const double distance, double &dest_lat, double &dest_lon)
+{
+    double origin_lat_rad = deg_to_rad(orig_lat);
+    double origin_lon_rad = deg_to_rad(orig_lon);
+    double bearing_rad = deg_to_rad(bearing);
+
+    double dr = distance / earthradius();
+
+    dest_lat =
+        asin(sin(origin_lat_rad) * cos(dr) + cos(origin_lat_rad) * sin(dr) * cos(bearing_rad));
+    dest_lon = origin_lon_rad + atan2(sin(bearing_rad) * sin(dr) * cos(origin_lat_rad),
+                                      cos(dr) - sin(origin_lat_rad) * sin(origin_lat_rad));
+    dest_lat = rad_to_deg(dest_lat);
+    dest_lon = wrap_valid_longitude(rad_to_deg(dest_lon));
+}
+
+// origin_lat in degrees
+// origin_lon in degrees
+// bearing in degrees
+// distance in metres
+void gps_offset(double orig_lat, double orig_lon, double east, double north, double &dest_lat,
+                double &dest_lon)
+{
+    double bearing = rad_to_deg(atan2(east, north));
+    double distance = sqrt(east * east + north * north);
+    gps_newpos(orig_lat, orig_lon, bearing, distance, dest_lat, dest_lon);
+}
+
+double altitude_from_pressure_delta(double gnd_abs_press, double gnd_temp, double press_abs,
+                                    double temp UNUSED)
+{
+    double scaling = press_abs / gnd_abs_press;
+    return 153.8462 * (gnd_temp + 273.15) * (1.0 - exp(0.190259 * log(scaling)));
+}

sololink/flightcode/dataflash_logger/analyzer_util.h

@@ -0,0 +1,75 @@
+#ifndef _ANALYZER_UTIL
+#define _ANALYZER_UTIL
+
+#include <string>
+#include <string.h>
+
+// from: http://stackoverflow.com/questions/2342162/stdstring-formatting-like-sprintf
+#include <memory>
+
+double earthradius();
+
+double wrap_valid_longitude(const double longitude);
+
+// http://www.movable-type.co.uk/scripts/latlong.html
+void gps_newpos(const double orig_lat, const double orig_lon, const double bearing,
+                const double distance, double &dest_lat, double &dest_lon);
+
+// origin_lat in degrees
+// origin_lon in degrees
+// bearing in degrees
+// distance in metres
+void gps_offset(double orig_lat, double orig_lon, double east, double north, double &dest_lat,
+                double &dest_lon);
+
+double altitude_from_pressure_delta(double gnd_abs_press, double gnd_temp, double press_abs,
+                                    double temp);
+
+template < typename... Args >
+std::string string_format(const char *format, const Args... args)
+{
+
+    int32_t size = snprintf(nullptr, 0, format, args...);
+    if (size < 0) {
+        ::fprintf(stderr, "snprintf error (%d): %s\n", size, strerror(errno));
+        abort();
+    }
+    size += 1; // Extra space for '\0'
+    std::unique_ptr< char[] > buf(new char[size]);
+    snprintf(buf.get(), size, format, args...);
+    return std::string(buf.get(), buf.get() + size - 1); // We don't want the '\0' inside
+}
+
+template < typename... Args >
+std::string string_format(const std::string format, const Args... args)
+{
+    return string_format(format.c_str(), args...);
+}
+
+#ifndef streq
+#define streq(x, y) (!strcmp(x, y))
+#endif
+
+// inline float deg_to_rad(const float deg) {
+//     return deg/M_PI * 180;
+// }
+
+// inline float rad_to_deg(const float rad) {
+//     return rad*180/M_PI;
+// }
+
+#define deg_to_rad(x) ((x)*M_PI / 180.0f)
+#define rad_to_deg(x) ((x)*180.0f / M_PI)
+
+#define is_zero(x) (x < 0.00001)
+#define is_equal(x, y) (is_zero(fabs((x) - (y))))
+
+void format_timestamp(char *buf, uint8_t buflen, uint64_t T);
+
+uint64_t now();
+
+double vec_len(double vec[3]);
+
+#define UNUSED __attribute__((unused))
+
+#endif // _ANALYZER_UTIL

sololink/flightcode/dataflash_logger/analyzervehicle_copter.cpp

@@ -0,0 +1,127 @@
+#include "analyzervehicle_copter.h"
+
+using namespace AnalyzerVehicle;
+
+bool Copter::param_default(const char *name, float &ret)
+{
+    if (_frame_type == frame_type_quad) {
+        if (_param_defaults_quad[name]) {
+            ret = _param_defaults_quad[name];
+            return true;
+        }
+    }
+    if (_param_defaults[name]) {
+        ret = _param_defaults[name];
+        return true;
+    }
+    return Base::param_default(name, ret);
+}
+
+/* I think there's an argument for moving the following into Analyzer: */
+
+bool Copter::is_flying()
+{
+    if (!is_armed()) {
+        // we hope we're not flying, anyway!
+        return false;
+    }
+
+    if (!any_motor_running_fast()) {
+        return false;
+    }
+
+    return true;
+}
+
+bool Copter::any_motor_running_fast()
+{
+    for (uint8_t i = 1; i < _num_motors; i++) {
+        if (_servo_output[i] > is_flying_motor_threshold) {
+            return true;
+        }
+    }
+    return false;
+}
+
+std::set< uint8_t > Copter::motors_clipping_high()
+{
+    std::set< uint8_t > ret;
+    char label[] = "RCx_MAX";
+    for (uint8_t i = 1; i <= _num_motors; i++) {
+        label[2] = '0' + i;
+        float max;
+        if (param(label, max)) {
+            uint16_t delta = abs((int32_t)_servo_output[i] - (uint16_t)max);
+            if ((float)delta / max < .05) { // within 5%
+                ret.insert(i);
+            }
+        }
+    }
+    return ret;
+}
+
+std::set< uint8_t > Copter::motors_clipping_low()
+{
+    std::set< uint8_t > ret;
+    char label[] = "RCx_MIN";
+    for (uint8_t i = 1; i <= _num_motors; i++) {
+        label[2] = '0' + i;
+        float min;
+        if (param(label, min)) {
+            if (_servo_output[i] < (uint16_t)min ||
+                _servo_output[i] - min < 105) { // FIXME: constant
+                ret.insert(i);
+            }
+            // uint16_t delta = abs((int32_t)_servo_output[i] - (uint16_t)min);
+            // ::fprintf(stderr, "%d delta=%d (%f)\n", i, delta, (float)delta/min);
+            // if ((float)delta/min < .05) {
+            //     ::fprintf(stderr, "%d clipping low \n", i);
+            //     ret.insert(i);
+            // }
+        }
+    }
+    return ret;
+}
+
+void Copter::set_frame_type(copter_frame_type frame_type)
+{
+    _frame_type = frame_type;
+    switch (frame_type) {
+    case frame_type_quad:
+        _num_motors = 4;
+        break;
+    case frame_type_y6:
+        _num_motors = 6;
+        break;
+    case invalid:
+        ::fprintf(stderr, "Invalid frame type");
+        abort();
+    }
+}
+
+bool Copter::exceeding_angle_max()
+{
+    float angle_max; // convert from centidegrees
+    if (param_with_defaults("ANGLE_MAX", angle_max)) {
+        angle_max /= 100;
+        if (fabs(att().roll()) > angle_max) {
+            return true;
+        }
+        if (fabs(att().pitch()) > angle_max) {
+            return true;
+        }
+    }
+    return false;
+}
+
+void Copter::set_frame(const char *frame_config_string)
+{
+    if (strstr(frame_config_string, "QUAD")) {
+        set_frame_type(AnalyzerVehicle::Copter::frame_type_quad);
+    } else if (strstr(frame_config_string, "Y6")) {
+        set_frame_type(AnalyzerVehicle::Copter::frame_type_y6);
+    } else {
+        ::fprintf(stderr, "Unknown frame (%s)\n", frame_config_string);
+        abort();
+    }
+}

sololink/flightcode/dataflash_logger/common_tool.cpp

@@ -0,0 +1,173 @@
+#include "common_tool.h"
+
+#include <errno.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include "la-log.h"
+
+void Common_Tool::init_config()
+{
+    if (config_filename == NULL) {
+        abort();
+    }
+    struct stat buf;
+    if (stat(config_filename, &buf) == -1) {
+        if (errno == ENOENT) {
+            if (!streq(default_config_filename, config_filename)) {
+                la_log(LOG_CRIT, "Config file (%s) does not exist", config_filename);
+                exit(1);
+            }
+        } else {
+            la_log(LOG_CRIT, "Failed to stat (%s): %s\n", config_filename, strerror(errno));
+            exit(1);
+        }
+    }
+    _config = new INIReader(config_filename);
+    if (_config == NULL) {
+        la_log(LOG_CRIT, "Failed to create config from (%s)\n", config_filename);
+        exit(1);
+    }
+    if (_config == NULL) {
+        _config = new INIReader("/dev/null");
+    }
+}
+
+void Common_Tool::check_fds_are_empty_after_select(fd_set &fds_read, fd_set &fds_write,
+                                                   fd_set &fds_err, uint8_t nfds)
+{
+    for (uint8_t i = 0; i < nfds; i++) {
+        if (FD_ISSET(i, &fds_read)) {
+            la_log(LOG_ERR, "fds_read not empty");
+            break;
+        }
+        if (FD_ISSET(i, &fds_write)) {
+            la_log(LOG_ERR, "fds_write not empty");
+            break;
+        }
+        if (FD_ISSET(i, &fds_err)) {
+            la_log(LOG_ERR, "fds_err not empty");
+            break;
+        }
+    }
+}
+
+void Common_Tool::pack_select_fds(fd_set &fds_read UNUSED, fd_set &fds_write UNUSED,
+                                  fd_set &fds_err UNUSED, uint8_t &nfds UNUSED)
+{
+}
+
+void Common_Tool::handle_select_fds(fd_set &fds_read UNUSED, fd_set &fds_write UNUSED,
+                                    fd_set &fds_err UNUSED, uint8_t &nfds UNUSED)
+{
+}
+
+void Common_Tool::sighup_received_tophalf()
+{
+}
+void Common_Tool::sighup_handler(int signal UNUSED)
+{
+    _sighup_received = true;
+}
+void Common_Tool::do_idle_callbacks()
+{
+}
+
+uint32_t Common_Tool::select_timeout_us()
+{
+    return 200000;
+}
+
+void Common_Tool::select_loop()
+{
+    fd_set fds_read;
+    fd_set fds_write;
+    fd_set fds_err;
+    uint8_t nfds;
+    while (1) {
+        if (_sighup_received) {
+            sighup_received_tophalf();
+            _sighup_received = false;
+        }
+        /* Wait for a packet, or time out if no packets arrive so we always
+           periodically log status and check for new destinations. Downlink
+           packets are on the order of 100/sec, so the timeout is such that
+           we don't expect timeouts unless solo stops sending packets. We
+           almost always get a packet with a 200 msec timeout, but not with
+           a 100 msec timeout. (Timeouts don't really matter though.) */
+
+        struct timeval timeout;
+
+        FD_ZERO(&fds_read);
+        FD_ZERO(&fds_write);
+        FD_ZERO(&fds_err);
+        nfds = 0;
+        pack_select_fds(fds_read, fds_write, fds_err, nfds);
+
+        timeout.tv_sec = 0;
+        timeout.tv_usec = select_timeout_us();
+        int res = select(nfds, &fds_read, &fds_write, &fds_err, &timeout);
+
+        if (res < 0) {
+            unsigned skipped = 0;
+            // if ((skipped = can_log_error()) >= 0)
+            la_log(LOG_ERR, "[%u] select: %s", skipped, strerror(errno));
+            /* this sleep is to avoid soaking the CPU if select starts
+               returning immediately for some reason */
+            /* previous code was not checking errfds; we are now, so
+               perhaps this usleep can go away -pb20150730 */
+            usleep(10000);
+            continue;
+        }
+
+        if (res == 0) {
+            // select timeout
+        }
+
+        handle_select_fds(fds_read, fds_write, fds_err, nfds);
+
+        check_fds_are_empty_after_select(fds_read, fds_write, fds_err, nfds);
+
+        do_idle_callbacks();
+    } /* while (1) */
+}
+
+void Common_Tool::parse_fd(Format_Reader *reader, int fd)
+{
+    char buf[1 << 16];
+    ssize_t buf_start = 0;
+    while (true) {
+        ssize_t bytes_read = read(fd, &buf[buf_start], sizeof(buf) - buf_start);
+        if (bytes_read == -1) {
+            fprintf(stderr, "Read failed: %s\n", strerror(errno));
+            exit(1);
+        }
+        if (bytes_read == 0) {
+            while (reader->feed((uint8_t *)buf, buf_start + bytes_read)) {
+            }
+            break;
+        }
+
+        bytes_read += buf_start;
+        ssize_t total_bytes_used = 0;
+        while (total_bytes_used < bytes_read) {
+            ssize_t bytes_used =
+                reader->feed((uint8_t *)(&buf[total_bytes_used]), bytes_read - total_bytes_used);
+            if (bytes_used > bytes_read - total_bytes_used) {
+                abort();
+            }
+            if (bytes_used == 0) {
+                break;
+            }
+            total_bytes_used += bytes_used;
+        }
+        // ::fprintf(stderr, "total_bytes_used = %u\n", total_bytes_used);
+        // ::fprintf(stderr, "bytes_read = %u\n", bytes_read);
+        memcpy(&buf[0], (uint8_t *)(&(buf[total_bytes_used])), bytes_read - total_bytes_used);
+        buf_start = bytes_read - total_bytes_used;
+    }
+
+    reader->end_of_log();
+
+    // ::fprintf(stderr, "Packet count: %d\n", packet_count);
+}

sololink/flightcode/dataflash_logger/common_tool.h

@@ -0,0 +1,44 @@
+#include "util.h"
+#include "analyzer_util.h"
+#include "INIReader.h"
+
+#include "format_reader.h"
+
+class Common_Tool
+{
+public:
+    Common_Tool() : config_filename(default_config_filename)
+    {
+    }
+
+    void sighup_handler(int signal);
+
+    void parse_fd(Format_Reader *reader, int fd);
+
+protected:
+    class INIReader *config()
+    {
+        return _config;
+    };
+    void init_config();
+    const char *default_config_filename = "/etc/sololink.conf";
+    const char *config_filename;
+
+    virtual void sighup_received_tophalf();
+
+    bool _sighup_received = false; // FIXME: scope
+
+    virtual uint32_t select_timeout_us();
+    void select_loop();
+
+    virtual void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                 uint8_t &nfds);
+    virtual void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                   uint8_t &nfds);
+    virtual void do_idle_callbacks();
+
+private:
+    void check_fds_are_empty_after_select(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                          uint8_t nfds);
+    class INIReader *_config = NULL;
+};

sololink/flightcode/dataflash_logger/dataflash_logger.cpp

@@ -0,0 +1,477 @@
+#include "dataflash_logger.h"
+
+#include <signal.h>
+#include <stdio.h> // for snprintf
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sched.h> // for sched_yield
+
+#include "la-log.h"
+#include "util.h"
+#include "../mavlink/c_library/common/mavlink.h"
+
+/*
+
+There are currently two threads: a "network" thread (see handle_message), and the fileio thread (see
+fileio_task).  The network thread takes the data from the packets and packs it into _write_buffers.
+It then passes the buffers to the fileio thread for writing to disk.  The buffers are then passed
+back to the network thread.
+
+Notes on thread safety:
+
+la_log is called by both the network and IO threads.  openlog/syslog are thread safe.  The la_log
+code is not designed to be thread safe.  That code is commented as such, and in future locking may
+be required there.  The current implementation has races concerning message suppression but fixing
+these may lead to more complexity than is justified.
+
+The fileio and network threads in dataflash logger share:
+ - out_fd
+ - _write_buffers[]
+
+out_fd is not to be touched in the main thread after it has been opened - unless logging has
+stopped, and all the buffers are owned by the network thread.  The fileio thread may only touch
+out_fd and _write_buffers if logging has started, or it owns buffers.
+
+The buffers contained in _write_buffers are owned by one thread at a time.  Only the owner may
+modify the buffer.  Ownership is transfered by assignment of the owner variable.  This will work for
+two threads, but locking of this transfer will be required if more threads become involved, and the
+programmer is suitably paranoid.
+
+*/
+
+void *fileio_task_wrapper(void *instance)
+{
+    ((DataFlash_Logger *)instance)->fileio_task();
+    return NULL;
+}
+
+DataFlash_Logger::DataFlash_Logger(MAVLink_Writer *mavlink_writer)
+    : MAVLink_Message_Handler(), _mavlink_writer(mavlink_writer),
+      target_system_id(target_system_id_default), target_component_id(target_component_id_default),
+      seqno_gap_nack_threshold(20)
+{
+    _current_write_buffer = &_write_buffers[0];
+    pthread_mutex_init(&_write_buffer_mutex, NULL);
+    pthread_cond_init(&_wakeup_fileio_thread, NULL);
+    pthread_create(&_fileio_thread, NULL, fileio_task_wrapper, this);
+}
+
+void DataFlash_Logger::write_buffer_to_disk(const struct df_write_buffer *buffer)
+{
+    const uint8_t length = MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN;
+    for (uint16_t j = 0; j < buffer->next_defered_write; j++) {
+        const struct defered_write *some_write = &buffer->defered_writes[j];
+        const uint32_t seqno = some_write->seqno;
+        const uint8_t *data = some_write->data;
+        if (lseek(out_fd, seqno * length, SEEK_SET) == -1) {
+            la_log(LOG_ERR, "mh-dfl: lseek (%u) failed: %s", seqno * length, strerror(errno));
+            return;
+        }
+        ssize_t written = write(out_fd, data, length);
+        if (written < length) {
+            la_log(LOG_ERR, "mh-dfl: short write: %s", strerror(errno));
+            return;
+        }
+    }
+
+    // TODO: write all of the blocks in the buffer to disk, trying to
+    // coalesce writes as we go:
+}
+
+void DataFlash_Logger::fileio_task()
+{
+    while (true) {
+        // we should never be called while fd == -1.
+        pthread_cond_wait(&_wakeup_fileio_thread, &_write_buffer_mutex);
+        // we don't actually need the mutex:
+        pthread_mutex_unlock(&_write_buffer_mutex);
+
+        df_write_buffer *buffer = next_write_buffer(FILEIO_THREAD);
+        while (buffer != NULL) {
+            write_buffer_to_disk(buffer);
+            buffer->next_defered_write = 0;
+            // hand the buffer back to the net thread:
+            buffer->owner = NETWORK_THREAD;
+            buffer = next_write_buffer(FILEIO_THREAD);
+            fsync(out_fd);
+
+            uint64_t now = clock_gettime_us(CLOCK_MONOTONIC);
+            if (now - fileio_last_log_time > 10000000) {
+                // the following isn't quite right given we seek around...
+                la_log(LOG_INFO, "mh-dfl: Current log size: %lu", lseek(out_fd, 0, SEEK_CUR));
+                fileio_last_log_time = now;
+            }
+        }
+    }
+}
+
+void DataFlash_Logger::sighup_received()
+{
+    logging_stop();
+}
+
+void DataFlash_Logger::idle_tenthHz()
+{
+}
+
+void DataFlash_Logger::idle_1Hz()
+{
+    if (!logging_started) {
+        if (sender_system_id != 0) {
+            // we've previously been logging, so telling the other end
+            // to stop logging may let us restart logging sooner
+            send_stop_logging_packet();
+        }
+        send_start_logging_packet();
+    }
+}
+void DataFlash_Logger::idle_10Hz()
+{
+    if (logging_started) {
+        // if no data packet in 10 seconds then close log
+        uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
+        if (now_us - _last_data_packet_time > 10000000) {
+            la_log(LOG_INFO, "mh-dfl: No data packets received for some time (now=%llu last=%llu). "
+                             " Closing log.  Final log size: %lu",
+                   now_us, _last_data_packet_time, lseek(out_fd, 0, SEEK_CUR));
+            logging_stop();
+        }
+    }
+}
+
+void DataFlash_Logger::idle_100Hz()
+{
+    push_response_queue();
+}
+
+void DataFlash_Logger::send_response(uint32_t seqno, bool status)
+{
+    mavlink_message_t msg;
+    mavlink_msg_remote_log_block_status_pack(system_id, component_id, &msg, sender_system_id,
+                                             sender_component_id, seqno, status);
+    _mavlink_writer->send_message(msg);
+}
+
+void DataFlash_Logger::push_response_queue()
+{
+    const uint8_t max_packets_to_send = 5;
+    uint8_t packets_sent = 0;
+    while (response_queue_head != response_queue_tail && packets_sent < max_packets_to_send) {
+        send_response(responses[response_queue_tail].seqno, responses[response_queue_tail].status);
+        response_queue_tail++;
+        if (response_queue_tail >= RESPONSE_QUEUE_LENGTH) {
+            response_queue_tail = 0;
+        }
+    }
+}
+
+bool DataFlash_Logger::configure(INIReader *config)
+{
+    if (!MAVLink_Message_Handler::configure(config)) {
+        return false;
+    }
+
+    std::string path = config->Get("dflogger", "log_dirpath", "/log/dataflash");
+    _log_directory_path = strdup(path.c_str());
+    if (_log_directory_path == NULL) {
+        return false;
+    }
+
+    // need to do equivalent of 'mkdir -p log_dirpath' here
+    // (i.e. handle multi-level path)
+    if (mkdir(_log_directory_path,
+              S_IRUSR | S_IWUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH) != 0) {
+        // this is expected to succeed on the first boot after a factory or
+        // settings reset, then fail with EEXIST each startup after that.
+        if (errno != EEXIST) {
+            la_log(LOG_ERR, "mh-dfl: Failed to create (%s): %s", _log_directory_path,
+                   strerror(errno));
+            return false;
+        }
+    }
+
+    target_system_id = config->GetInteger("dflogger", "target_system_id", 0);
+    target_component_id = config->GetInteger("dflogger", "target_component_id", 0);
+
+    return true;
+}
+
+bool DataFlash_Logger::make_new_log_filename(char *buffer, uint8_t bufferlen)
+{
+    uint8_t lastlog_buflen = 128;
+    char lastlog_buf[128];
+    // this was really beautiful, but I don't think SoloLink has an
+    // RTC; it depends on GPS to update its clock (scribbled down
+    // periodically?)
+
+    // time_t t;
+
+    // time(&t);
+    // struct tm *timebits = gmtime(&t);
+
+    // snprintf(buffer, bufferlen, "%s/%04d%02d%02d%02d%02d%02d.BIN",
+    //          _log_directory_path,
+    //          timebits->tm_year+1900,
+    //          timebits->tm_mon+1,
+    //          timebits->tm_mday,
+    //          timebits->tm_hour,
+    //          timebits->tm_min,
+    //          timebits->tm_sec);
+
+    memset(lastlog_buf, '\0', lastlog_buflen);
+    snprintf(lastlog_buf, lastlog_buflen, "%s/LASTLOG.TXT", _log_directory_path);
+    int fd;
+    uint32_t num;
+    if ((fd = open(lastlog_buf, O_RDONLY)) == -1) {
+        if (errno != ENOENT) {
+            // what?
+            syslog(LOG_ERR, "Failed to open (%s) for reading: %s", lastlog_buf, strerror(errno));
+            return false;
+        }
+        num = 1;
+    } else {
+        uint8_t numbuf_len = 128;
+        char numbuf[numbuf_len];
+        memset(numbuf, '\0', numbuf_len);
+        int bytes_read = read(fd, numbuf, numbuf_len);
+        close(fd);
+        if (bytes_read == -1) {
+            return false;
+        }
+        num = strtoul(numbuf, NULL, 10);
+        num++;
+    }
+
+    if ((fd = open(lastlog_buf, O_WRONLY | O_TRUNC | O_CREAT)) == -1) {
+        // *shrug*  We will continue to overwrite, I guess...
+    } else {
+        const uint8_t outsize = 16;
+        char out[outsize];
+        memset(out, '\0', outsize);
+        int towrite = snprintf(out, outsize, "%d\r\n", num);
+        write(fd, out, towrite); // ignore return...
+        close(fd);
+    }
+
+    snprintf(buffer, bufferlen, "%s/%d.BIN", _log_directory_path, num);
+
+    return true;
+}
+
+bool DataFlash_Logger::output_file_open()
+{
+    const uint8_t filename_length = 64;
+    char filename[filename_length];
+
+    if (!make_new_log_filename(filename, filename_length)) {
+        return false;
+    }
+
+    out_fd = open(filename, O_WRONLY | O_CREAT | O_TRUNC, 0777);
+    if (out_fd == -1) {
+        printf("Failed to open (%s): %s\n", filename, strerror(errno));
+        la_log(LOG_ERR, "mh-dfl: Failed to open (%s): %s", filename, strerror(errno));
+        return false;
+    }
+    la_log(LOG_INFO, "mh-dfl: Opened log file (%s)", filename);
+
+    return true;
+}
+
+void DataFlash_Logger::output_file_close()
+{
+    close(out_fd);
+    out_fd = -1;
+}
+
+void DataFlash_Logger::queue_response(uint32_t seqno, bool status)
+{
+    responses[response_queue_head].seqno = seqno;
+    responses[response_queue_head].status = status;
+    response_queue_head++;
+    if (response_queue_head >= RESPONSE_QUEUE_LENGTH) {
+        response_queue_head = 0;
+    }
+}
+
+void DataFlash_Logger::queue_ack(uint32_t seqno)
+{
+    queue_response(seqno, true);
+}
+
+void DataFlash_Logger::queue_nack(uint32_t seqno)
+{
+    queue_response(seqno, false);
+}
+
+void DataFlash_Logger::queue_gap_nacks(uint32_t seqno)
+{
+    if (seqno <= highest_seqno_seen) {
+        // this packet filled in a gap (or was a dupe)
+        return;
+    }
+
+    if (seqno - highest_seqno_seen > seqno_gap_nack_threshold) {
+        // we've seen some serious disruption, and lots of stuff
+        // is probably going to be lost.  Do not bother NACKing
+        // packets here and let the server sort things out
+        return;
+    }
+
+    for (uint32_t i = highest_seqno_seen + 1; i < seqno; i++) {
+        queue_nack(i);
+    }
+}
+
+bool DataFlash_Logger::logging_start(mavlink_remote_log_data_block_t &msg UNUSED)
+{
+    sender_system_id = most_recent_sender_system_id;
+    sender_component_id = most_recent_sender_component_id;
+    la_log_unsuppress();
+    la_log(LOG_INFO, "mh-dfl: Starting log, target is (%d/%d), I am (%d/%d)", sender_system_id,
+           sender_component_id, this_system_id, this_component_id);
+    if (!output_file_open()) {
+        return false;
+    }
+
+    logging_started = true;
+    return true;
+}
+
+void DataFlash_Logger::ensure_write_buffers_ownership()
+{
+    for (uint16_t i = 0; i < _max_write_buffers; i++) {
+        uint8_t count = 0;
+        while (_write_buffers[i].owner != NETWORK_THREAD) {
+            pthread_cond_signal(&_wakeup_fileio_thread);
+            sched_yield();
+            // do we need really a sleep in here?
+            usleep(100000);
+            if (count++ > 100) {
+                la_log(LOG_ERR, "Stuck waiting for buffers to be freed; exitting");
+                abort();
+            }
+        }
+    }
+}
+
+void DataFlash_Logger::logging_stop()
+{
+    logging_started = false;
+    // we need to own all of the _write_buffers[] to ensure the
+    // iothread is not currently using the output filehandle:
+    ensure_write_buffers_ownership();
+    output_file_close();
+}
+
+void DataFlash_Logger::handle_message(uint64_t timestamp, mavlink_message_t &msg)
+{
+    most_recent_sender_system_id = msg.sysid;
+    most_recent_sender_component_id = msg.compid;
+    MAVLink_Message_Handler::handle_message(timestamp, msg);
+}
+
+struct DataFlash_Logger::df_write_buffer *
+DataFlash_Logger::next_write_buffer(const DataFlash_Logger::write_buffer_owner owner)
+{
+    for (uint8_t i = 0; i < _max_write_buffers; i++) {
+        if (_write_buffers[i].owner == owner) {
+            return &_write_buffers[i];
+        }
+    }
+    return NULL;
+}
+
+void DataFlash_Logger::handle_decoded_message(uint64_t T UNUSED,
+                                              mavlink_remote_log_data_block_t &msg)
+{
+    if (!logging_started) {
+        if (msg.seqno == 0) {
+            if (!logging_start(msg)) {
+                return;
+            }
+        } else {
+            return;
+        }
+    }
+
+    uint64_t now = clock_gettime_us(CLOCK_MONOTONIC);
+    if (now - _last_data_packet_time > 100000) {
+        ::la_log(LOG_ERR, "mh-dfl: long time between messages (%ld)", now - _last_data_packet_time);
+    }
+
+    if (_current_write_buffer == NULL) {
+        _current_write_buffer = next_write_buffer(NETWORK_THREAD);
+        if (_current_write_buffer == NULL) {
+            la_log(LOG_ERR, "mh-dfl: no write buffer available");
+            _last_data_packet_time = now;
+            return;
+        }
+    }
+
+    struct defered_write *defered_write =
+        &_current_write_buffer->defered_writes[_current_write_buffer->next_defered_write++];
+    memcpy(defered_write->data, msg.data, MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN);
+    defered_write->seqno = msg.seqno;
+    if (_current_write_buffer->next_defered_write >= _df_write_buffer_max_defered_writes) {
+        // this buffer is full.  Hand it to the FILEIO thread:
+        _current_write_buffer->owner = FILEIO_THREAD;
+        _current_write_buffer = NULL;
+        // and wake the fileio thread up:
+        pthread_cond_signal(&_wakeup_fileio_thread);
+    }
+
+    if (clock_gettime_us(CLOCK_MONOTONIC) - now > 100000) {
+        ::la_log(LOG_ERR, "mh-dfl: long time to write (%ld)",
+                 clock_gettime_us(CLOCK_MONOTONIC) - now);
+    }
+
+    // queue an ack for this packet
+    queue_ack(msg.seqno);
+
+    // queue nacks for gaps
+    queue_gap_nacks(msg.seqno);
+
+    if (msg.seqno > highest_seqno_seen) {
+        if (msg.seqno - highest_seqno_seen > 100) {
+            la_log(LOG_ERR, "mh-dfl: large seqno gap: %ld", msg.seqno - highest_seqno_seen);
+        }
+        highest_seqno_seen = msg.seqno;
+    }
+
+    _last_data_packet_time = now;
+}
+
+void DataFlash_Logger::send_start_logging_packet()
+{
+    send_start_or_stop_logging_packet(true);
+}
+
+void DataFlash_Logger::send_stop_logging_packet()
+{
+    send_start_or_stop_logging_packet(false);
+}
+
+void DataFlash_Logger::send_start_or_stop_logging_packet(bool is_start)
+{
+    mavlink_message_t msg;
+
+    uint8_t system_id = is_start ? target_system_id : sender_system_id;
+    uint8_t component_id = is_start ? target_component_id : sender_component_id;
+    uint32_t magic_number;
+    if (is_start) {
+        la_log(LOG_INFO, "mh-dfl: sending start packet to (%d/%d)", system_id, component_id);
+        magic_number = MAV_REMOTE_LOG_DATA_BLOCK_START;
+    } else {
+        la_log(LOG_INFO, "mh-dfl: sending stop packet to (%d/%d)", system_id, component_id);
+        magic_number = MAV_REMOTE_LOG_DATA_BLOCK_STOP;
+    }
+    mavlink_msg_remote_log_block_status_pack(this_system_id, this_component_id, &msg, system_id,
+                                             component_id, magic_number, 1);
+
+    _mavlink_writer->send_message(msg);
+}

sololink/flightcode/dataflash_logger/dataflash_logger.h

@@ -0,0 +1,119 @@
+#ifndef DATAFLASH_LOGGER_H
+#define DATAFLASH_LOGGER_H
+
+/*
+ * dataflash_logger
+ *
+ * Receive telemetry (mavlink) via UDP from Solo, and create dataflash log files
+ *
+ * Initiate a remote-dataflash stream
+ */
+
+#include "INIReader.h"
+
+#include "mavlink_message_handler.h"
+#include "mavlink_writer.h"
+
+#include "../mavlink/c_library/common/mavlink.h"
+
+class DataFlash_Logger : public MAVLink_Message_Handler
+{
+
+public:
+    void fileio_task();
+
+    DataFlash_Logger(MAVLink_Writer *mavlink_writer);
+
+    enum write_buffer_owner { NETWORK_THREAD, FILEIO_THREAD };
+
+private:
+    bool configure(INIReader *config);
+
+    void sighup_received();
+
+    void idle_tenthHz();
+    void idle_1Hz();
+    void idle_10Hz();
+    void idle_100Hz();
+    bool logging_start(mavlink_remote_log_data_block_t &msg);
+    void logging_stop();
+    void send_stop_logging_packet();
+
+    bool output_file_open();
+    void output_file_close();
+
+    void ensure_write_buffers_ownership();
+
+    // more, smaller buffers means a lower latency to disk.  A total
+    // of 700 defered writes gives about 10 seconds of buffering based
+    // on a 12k transfer rate.
+    static const uint8_t _max_write_buffers = 7;
+    static const uint16_t _df_write_buffer_max_defered_writes = 100;
+    struct defered_write {
+        uint8_t data[MAVLINK_MSG_REMOTE_LOG_DATA_BLOCK_FIELD_DATA_LEN];
+        uint32_t seqno;
+    };
+
+    struct df_write_buffer {
+        struct defered_write defered_writes[_df_write_buffer_max_defered_writes];
+        write_buffer_owner owner = NETWORK_THREAD;
+        uint16_t next_defered_write = 0;
+    } _write_buffers[_max_write_buffers];
+    struct df_write_buffer *_current_write_buffer = NULL;
+    void write_buffer_to_disk(const struct df_write_buffer *buffer);
+    bool write_block_for_seqno(const uint8_t *data, uint32_t seqno);
+    struct df_write_buffer *next_write_buffer(const write_buffer_owner owner);
+    uint64_t fileio_last_log_time = 0;
+
+    pthread_mutex_t _write_buffer_mutex;
+    pthread_cond_t _wakeup_fileio_thread;
+    pthread_t _fileio_thread;
+
+    MAVLink_Writer *_mavlink_writer = NULL;
+    uint8_t this_system_id = 57;
+    uint8_t this_component_id = 57;
+
+    const uint8_t target_system_id_default = 0;
+    const uint8_t target_component_id_default = 0;
+    uint8_t most_recent_sender_system_id;
+    uint8_t most_recent_sender_component_id;
+    uint8_t target_system_id;        // who to send our request-for-logs to
+    uint8_t target_component_id;     // who to send our request-for-logs to
+    uint8_t sender_system_id = 0;    // who the logs areactually coming from
+    uint8_t sender_component_id = 0; // who the logs areactually coming from
+
+    void send_start_logging_packet();
+    void send_start_or_stop_logging_packet(bool is_start);
+    const char *_log_directory_path;
+    int out_fd;
+    bool logging_started = false;
+
+    void handle_message(uint64_t timestamp, mavlink_message_t &msg);
+    void handle_decoded_message(uint64_t T, mavlink_remote_log_data_block_t &msg);
+
+    bool make_new_log_filename(char *buffer, uint8_t bufferlen);
+
+    void send_response(uint32_t seqno, bool status);
+    void push_response_queue();
+
+#define RESPONSE_QUEUE_LENGTH 128
+    struct packet_status {
+        uint32_t seqno;
+        bool status; // ack == true
+    } responses[RESPONSE_QUEUE_LENGTH];
+    uint8_t response_queue_head = 0;
+    uint8_t response_queue_tail = 0;
+
+    uint32_t highest_seqno_seen = 0;
+    uint64_t _last_data_packet_time = 0;
+
+    /* if we lose > this many packets we do not nack anything in that gap: */
+    uint8_t seqno_gap_nack_threshold;
+
+    void queue_response(uint32_t seqno, bool status);
+    void queue_nack(uint32_t seqno);
+    void queue_ack(uint32_t seqno);
+    void queue_gap_nacks(uint32_t seqno);
+};
+
+#endif

sololink/flightcode/dataflash_logger/dataflash_logger_program.cpp

@@ -0,0 +1,167 @@
+#include "dataflash_logger_program.h"
+
+#include "dataflash_logger.h"
+#include "heart.h"
+#include "mavlink_reader.h"
+#include "telem_forwarder_client.h"
+#include "telem_serial.h"
+
+#include "la-log.h"
+
+#include <signal.h>
+#include <unistd.h>
+
+const char *DataFlash_Logger_Program::program_name()
+{
+    if (_argv == NULL) {
+        return "[Unknown]";
+    }
+    return _argv[0];
+}
+
+void DataFlash_Logger_Program::usage()
+{
+    ::printf("Usage:\n");
+    ::printf("%s [OPTION] [FILE]\n", program_name());
+    ::printf(" -c filepath      use config file filepath\n");
+    ::printf(" -h               display usage information\n");
+    ::printf(" -d               debug mode\n");
+    ::printf("\n");
+    ::printf("Example: %s\n", program_name());
+    exit(0);
+}
+
+DataFlash_Logger_Program logger;
+
+void sighup_handler(int signal)
+{
+    logger.sighup_handler(signal);
+}
+
+void DataFlash_Logger_Program::do_idle_callbacks()
+{
+    reader->do_idle_callbacks();
+}
+
+void DataFlash_Logger_Program::sighup_received_tophalf()
+{
+    reader->sighup_handler();
+}
+
+uint32_t DataFlash_Logger_Program::select_timeout_us()
+{
+    if (_writer->any_data_to_send()) {
+        return 0;
+    }
+    return Common_Tool::select_timeout_us();
+}
+
+void DataFlash_Logger_Program::pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                               uint8_t &nfds)
+{
+    client->pack_select_fds(fds_read, fds_write, fds_err, nfds);
+}
+
+void DataFlash_Logger_Program::do_writer_sends()
+{
+    client->do_writer_sends();
+}
+
+void DataFlash_Logger_Program::handle_select_fds(fd_set &fds_read, fd_set &fds_write,
+                                                 fd_set &fds_err, uint8_t &nfds)
+{
+    client->handle_select_fds(fds_read, fds_write, fds_err, nfds);
+
+    // FIXME: find a more interesting way of doing this...  we should
+    // probably rejig things so that the client is a mavlink_reader
+    // and simply produces mavlink_message_t's itself, rather than us
+    // handing off the a dedicated parser object here.
+    reader->feed(client->_recv_buf, client->_recv_buflen_content);
+    client->_recv_buflen_content = 0;
+
+    // handle data *to* e.g. telem_forwarder
+    do_writer_sends();
+}
+
+void DataFlash_Logger_Program::run()
+{
+    init_config();
+
+    if (!debug_mode) {
+        la_log_syslog_open();
+    }
+
+    la_log(LOG_INFO, "dataflash_logger starting: built " __DATE__ " " __TIME__);
+    signal(SIGHUP, ::sighup_handler);
+
+    reader = new MAVLink_Reader(config());
+    if (reader == NULL) {
+        la_log(LOG_ERR, "Failed to create reader from (%s)\n", config_filename);
+        exit(1);
+    }
+
+    if (serial_port) {
+        client = new Telem_Serial(_client_recv_buf, sizeof(_client_recv_buf));
+    } else {
+        client = new Telem_Forwarder_Client(_client_recv_buf, sizeof(_client_recv_buf));
+    }
+    client->configure(config());
+    client->init();
+
+    _writer = new MAVLink_Writer(config());
+    _writer->add_client(client);
+    if (_writer == NULL) {
+        la_log(LOG_ERR, "Failed to create writer from (%s)\n", config_filename);
+        exit(1);
+    }
+
+    // instantiate message handlers:
+    DataFlash_Logger *dataflash_logger = new DataFlash_Logger(_writer);
+    if (dataflash_logger != NULL) {
+        reader->add_message_handler(dataflash_logger, "DataFlash_Logger");
+    } else {
+        la_log(LOG_INFO, "Failed to create dataflash logger");
+    }
+
+    Heart *heart = new Heart(_writer);
+    if (heart != NULL) {
+        reader->add_message_handler(heart, "Heart");
+    } else {
+        la_log(LOG_INFO, "Failed to create heart");
+    }
+
+    return select_loop();
+}
+
+void DataFlash_Logger_Program::parse_arguments(int argc, char *argv[])
+{
+    int opt;
+    _argc = argc;
+    _argv = argv;
+
+    while ((opt = getopt(argc, argv, "hc:ds")) != -1) {
+        switch (opt) {
+        case 'h':
+            usage();
+            break;
+        case 'c':
+            config_filename = optarg;
+            break;
+        case 'd':
+            debug_mode = true;
+            break;
+        case 's':
+            serial_port = true;
+            break;
+        }
+    }
+}
+
+/*
+* main - entry point
+*/
+int main(int argc, char *argv[])
+{
+    logger.parse_arguments(argc, argv);
+    logger.run();
+}

sololink/flightcode/dataflash_logger/dataflash_logger_program.h

@@ -0,0 +1,49 @@
+#include "common_tool.h"
+
+#include "mavlink_reader.h"
+#include "mavlink_writer.h"
+#include "telem_client.h"
+
+class DataFlash_Logger_Program : public Common_Tool
+{
+public:
+    DataFlash_Logger_Program() : Common_Tool()
+    {
+    }
+
+    void run();
+
+    void parse_arguments(int argc, char *argv[]);
+    const char *program_name();
+
+    void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                         uint8_t &nfds) override;
+    void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                           uint8_t &nfds) override;
+
+private:
+    void usage();
+    void sighup_received_tophalf() override;
+    void do_idle_callbacks() override;
+    uint32_t select_timeout_us() override;
+
+    void do_writer_sends();
+
+    MAVLink_Reader *reader;
+    MAVLink_Writer *_writer;
+
+    long _argc = 0;
+    char **_argv = NULL;
+
+    uint8_t _client_recv_buf[512] = {}; // FIXME constant was TELEM_PKT_MAX
+
+    static const uint32_t _client_buflen = 65536; // FIXME constant
+    uint32_t _client_buflen_start = 0;
+    uint32_t _client_buflen_stop = 0;
+
+    Telem_Client *client = NULL;
+    bool debug_mode = false;
+    bool serial_port = false;
+    // uint8_t _writer_buf[_writer_buflen] = { };
+    uint32_t canary = 9876543;
+};

sololink/flightcode/dataflash_logger/dataflash_message_handler.cpp

@@ -0,0 +1 @@
+#include "dataflash_message_handler.h"

sololink/flightcode/dataflash_logger/dataflash_message_handler.h

@@ -0,0 +1,31 @@
+#ifndef DATAFLASH_MESSAGE_HANDLER_H
+#define DATAFLASH_MESSAGE_HANDLER_H
+
+#include <stdio.h>
+
+/*
+ * dataflash_message_handler
+ *
+ * A base class for objects which process dataflash messages and
+ * possibly send responses
+ *
+ */
+
+#include <stdint.h>
+
+#include "INIReader.h"
+
+#include "message_handler.h"
+
+class DataFlash_Message_Handler : public Message_Handler
+{
+public:
+    virtual void handle_format_message_received(const char *name, const struct log_Format &format,
+                                                const char *msg) = 0;
+    virtual void handle_message_received(const struct log_Format &format, const uint8_t *msg) = 0;
+
+protected:
+private:
+};
+
+#endif

sololink/flightcode/dataflash_logger/format_reader.cpp

@@ -0,0 +1,61 @@
+#include "format_reader.h"
+
+#include "la-log.h"
+
+void Format_Reader::do_idle_callbacks()
+{
+    uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
+    if (next_100hz_time <= now_us) {
+        for (int i = 0; i < next_message_handler; i++) {
+            message_handler[i]->idle_100Hz();
+        }
+        next_100hz_time += 10000;
+    }
+    if (next_10hz_time <= now_us) {
+        for (int i = 0; i < next_message_handler; i++) {
+            message_handler[i]->idle_10Hz();
+        }
+        next_10hz_time += 100000;
+    }
+    if (next_1hz_time <= now_us) {
+        for (int i = 0; i < next_message_handler; i++) {
+            message_handler[i]->idle_1Hz();
+        }
+        next_1hz_time += 1000000;
+    }
+    if (next_tenthhz_time <= now_us) {
+        for (int i = 0; i < next_message_handler; i++) {
+            message_handler[i]->idle_tenthHz();
+        }
+        next_tenthhz_time += 10000000;
+    }
+}
+
+bool Format_Reader::add_message_handler(Message_Handler *handler, const char *handler_name)
+{
+    if (MAX_MESSAGE_HANDLERS - next_message_handler < 2) {
+        la_log(LOG_INFO, "Insufficient message handler slots (MAX=%d) (next=%d)?!",
+               MAX_MESSAGE_HANDLERS, next_message_handler);
+        return false;
+    }
+
+    if (!handler->configure(_config)) {
+        la_log(LOG_INFO, "Failed to configure (%s)", handler_name);
+        return false;
+    }
+
+    message_handler[next_message_handler++] = handler;
+    return true;
+}
+
+void Format_Reader::clear_message_handlers()
+{
+    next_message_handler = 0;
+}
+
+void Format_Reader::sighup_handler()
+{
+    for (int i = 0; i < next_message_handler; i++) {
+        message_handler[i]->sighup_received();
+    }
+}

sololink/flightcode/dataflash_logger/format_reader.h

@@ -0,0 +1,49 @@
+#ifndef FORMAT_READER_H
+#define FORMAT_READER_H
+
+#include "INIReader.h"
+#include "util.h"
+
+#include "message_handler.h"
+
+class Format_Reader
+{
+public:
+    Format_Reader(INIReader *config) : _config(config)
+    {
+        uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
+        next_tenthhz_time = now_us;
+        next_1hz_time = now_us;
+        next_10hz_time = now_us;
+        next_100hz_time = now_us;
+    };
+
+    void do_idle_callbacks();
+
+    virtual bool add_message_handler(Message_Handler *handler, const char *handler_name);
+    virtual void clear_message_handlers();
+
+    virtual void sighup_handler();
+
+    virtual uint32_t feed(const uint8_t *buf, const uint32_t len) = 0;
+
+    virtual void end_of_log(){};
+
+protected:
+    INIReader *_config;
+
+// FIXME: Scope
+#define MAX_MESSAGE_HANDLERS 10
+    uint8_t next_message_handler = 0;
+    Message_Handler *message_handler[MAX_MESSAGE_HANDLERS];
+
+private:
+    bool sighup_received = false;
+
+    uint64_t next_tenthhz_time;
+    uint64_t next_1hz_time;
+    uint64_t next_10hz_time;
+    uint64_t next_100hz_time;
+};
+
+#endif

sololink/flightcode/dataflash_logger/heart.cpp

@@ -0,0 +1,39 @@
+#include "heart.h"
+
+#include "util.h"
+#include "la-log.h"
+
+void Heart::idle_10Hz()
+{
+    uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
+    if (last_heartbeat_time + heartbeat_interval < now_us) {
+        last_heartbeat_time = now_us;
+        beat();
+    }
+}
+
+bool Heart::configure(INIReader *config)
+{
+    if (!MAVLink_Message_Handler::configure(config)) {
+        return false;
+    }
+    return true;
+}
+
+void Heart::beat()
+{
+    mavlink_message_t msg;
+
+    uint8_t type = MAV_TYPE_GCS;
+    uint8_t autopilot = MAV_AUTOPILOT_INVALID;
+    uint8_t base_mode = 0;
+    uint32_t custom_mode = 0;
+    uint8_t system_status = 0;
+
+    la_log(LOG_DEBUG, "mh-h: sending heartbeat");
+
+    mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode,
+                               custom_mode, system_status);
+
+    _mavlink_writer->send_message(msg);
+}

sololink/flightcode/dataflash_logger/heart.h

@@ -0,0 +1,34 @@
+#ifndef HEART_H
+#define HEART_H
+
+/*
+ * heart
+ *
+ * Periodically send mavlink heartbeats to our upstream connection
+ *
+ */
+
+#include "mavlink_message_handler.h"
+#include "mavlink_writer.h"
+
+class Heart : public MAVLink_Message_Handler
+{
+
+public:
+    Heart(MAVLink_Writer *mavlink_writer)
+        : MAVLink_Message_Handler(), _mavlink_writer(mavlink_writer), last_heartbeat_time(0),
+          heartbeat_interval(5000000) // microseconds
+    {
+    }
+
+    void idle_10Hz();
+    bool configure(INIReader *config);
+
+private:
+    MAVLink_Writer *_mavlink_writer = NULL;
+    uint64_t last_heartbeat_time;
+    const uint32_t heartbeat_interval;
+    void beat();
+};
+
+#endif

sololink/flightcode/dataflash_logger/la-log.cpp

@@ -0,0 +1,82 @@
+#include "la-log.h"
+
+// Caution: the functions exposed in here are called by different
+// threads in the same process.
+
+LALog lalog;
+
+// globa functions for convenience
+void la_log_syslog_open()
+{
+    lalog.syslog_open();
+}
+
+void la_log_unsuppress()
+{
+    lalog.unsupress();
+}
+
+void la_log(int priority, const char *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    lalog.log_ap(priority, format, ap);
+    va_end(ap);
+}
+
+void LALog::syslog_open()
+{
+    openlog("dl", LOG_NDELAY, LOG_LOCAL1);
+
+    use_syslog = true;
+}
+
+void LALog::log(const int priority, const char *format, ...)
+{
+    va_list ap;
+    va_start(ap, format);
+    log_ap(priority, format, ap);
+    va_end(ap);
+}
+
+bool LALog::should_suppress()
+{
+    return _message_count_this_time_period >= _max_messages_per_time_period;
+}
+
+void LALog::unsupress()
+{
+    _suppressing = false;
+    _message_count_this_time_period = 0;
+    _time_period_start = time(NULL);
+    if (_suppressed_message_count) {
+        // hmmm.  recursion. hmmm.
+        log(LOG_ERR, "%d messages supressed", _suppressed_message_count);
+        _message_count_this_time_period--; // *cough* let one
+        // *message through
+        // *apart from us....
+        _suppressed_message_count = 0;
+    }
+}
+void LALog::log_ap(int priority, const char *format, va_list ap)
+{
+    if (time(NULL) - _time_period_start > _time_period) {
+        unsupress();
+    }
+    if (should_suppress()) {
+        _suppressing = true;
+    }
+    if (suppressing()) {
+        _suppressed_message_count++;
+        return;
+    }
+
+    _message_count_this_time_period++;
+
+    if (use_syslog) {
+        vsyslog(priority, format, ap);
+    } else {
+        vfprintf(stderr, format, ap);
+        fprintf(stderr, "%s", "\n");
+    }
+}

sololink/flightcode/dataflash_logger/la-log.h

@@ -0,0 +1,45 @@
+#ifndef _LA_LOG_H
+#define _LA_LOG_H
+
+#include <stdarg.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <syslog.h>
+#include <time.h>
+
+void la_log_syslog_open();
+void la_log(int priority, const char *format, ...);
+void la_log_unsuppress();
+
+class LALog
+{
+public:
+    LALog() : _time_period_start(time(NULL))
+    {
+    }
+    void syslog_open();
+
+    void log(int priority, const char *format, ...);
+    void log_ap(int priority, const char *format, va_list ap);
+
+    void unsupress();
+    bool should_suppress();
+    bool suppressing()
+    {
+        return _suppressing;
+    }
+
+private:
+    // really rough rate limiting for log messages.  We could keep a
+    // hash here of formats and selectively supress based on format.
+    bool _suppressing = false;
+    const uint8_t _time_period = 5; // seconds
+    const uint8_t _max_messages_per_time_period = 10;
+    uint32_t _suppressed_message_count = 0;
+    uint8_t _message_count_this_time_period = 0;
+    time_t _time_period_start;
+
+    bool use_syslog = false;
+};
+
+#endif

sololink/flightcode/dataflash_logger/mavlink_message_handler.cpp

@@ -0,0 +1,119 @@
+#include "mavlink_message_handler.h"
+
+#include "la-log.h"
+#include <errno.h>
+
+// void MAVLink_Message_Handler::send_message_to_telem_forwarder(mavlink_message_t &msg)
+// {
+//   uint8_t sendbuf[512];
+
+//   uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t*)sendbuf,&msg);
+
+//   if (sendto(_fd_telem_forwarder, sendbuf, messageLen, 0, (struct sockaddr
+//   *)_sa_telemetry_forwarder, sizeof(struct sockaddr)) == -1) {
+//     la_log(LOG_INFO, "Failed sendto: %s", strerror(errno));
+//   }
+// }
+
+void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg)
+{
+    // ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid);
+    switch (msg.msgid) {
+    case MAVLINK_MSG_ID_AHRS2: {
+        mavlink_ahrs2_t decoded;
+        mavlink_msg_ahrs2_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_ATTITUDE: {
+        mavlink_attitude_t decoded;
+        mavlink_msg_attitude_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_EKF_STATUS_REPORT: {
+        mavlink_ekf_status_report_t decoded;
+        mavlink_msg_ekf_status_report_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
+        mavlink_global_position_int_t decoded;
+        mavlink_msg_global_position_int_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_GPS_RAW_INT: {
+        mavlink_gps_raw_int_t decoded;
+        mavlink_msg_gps_raw_int_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_HEARTBEAT: {
+        mavlink_heartbeat_t decoded;
+        mavlink_msg_heartbeat_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_MOUNT_STATUS: {
+        mavlink_mount_status_t decoded;
+        mavlink_msg_mount_status_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
+        mavlink_nav_controller_output_t decoded;
+        mavlink_msg_nav_controller_output_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_PARAM_VALUE: {
+        mavlink_param_value_t decoded;
+        mavlink_msg_param_value_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: {
+        mavlink_remote_log_data_block_t decoded;
+        mavlink_msg_remote_log_data_block_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_SCALED_PRESSURE: {
+        mavlink_scaled_pressure_t decoded;
+        mavlink_msg_scaled_pressure_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_SCALED_PRESSURE2: {
+        mavlink_scaled_pressure2_t decoded;
+        mavlink_msg_scaled_pressure2_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
+        mavlink_servo_output_raw_t decoded;
+        mavlink_msg_servo_output_raw_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_STATUSTEXT: {
+        mavlink_statustext_t decoded;
+        mavlink_msg_statustext_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_SYS_STATUS: {
+        mavlink_sys_status_t decoded;
+        mavlink_msg_sys_status_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    case MAVLINK_MSG_ID_VFR_HUD: {
+        mavlink_vfr_hud_t decoded;
+        mavlink_msg_vfr_hud_decode(&msg, &decoded);
+        handle_decoded_message(timestamp, decoded);
+        break;
+    }
+    }
+}

sololink/flightcode/dataflash_logger/mavlink_message_handler.h

@@ -0,0 +1,98 @@
+#ifndef MAVLINK_MESSAGE_HANDLER_H
+#define MAVLINK_MESSAGE_HANDLER_H
+
+/*
+ * mavlink_message_handler
+ *
+ * A base class for objects which process mavlink messages and
+ * possibly send responses
+ *
+ */
+
+#include <stdint.h>
+
+#include "INIReader.h"
+#include "../mavlink/c_library/ardupilotmega/mavlink.h"
+#include "../mavlink/c_library/common/mavlink.h"
+
+#include <netinet/in.h>
+
+#include "message_handler.h"
+
+#define UNUSED __attribute__((unused))
+
+class MAVLink_Message_Handler : public Message_Handler
+{
+public:
+    MAVLink_Message_Handler() : Message_Handler()
+    {
+    }
+
+    virtual bool configure(INIReader *config)
+    {
+        system_id = config->GetInteger("dflogger", "system_id", 254);
+        component_id = config->GetInteger("dflogger", "component_id", 17);
+        return true;
+    }
+
+    virtual void handle_message(uint64_t timestamp, mavlink_message_t &msg);
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_ahrs2_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_attitude_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_ekf_status_report_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_gps_raw_int_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED,
+                                        mavlink_global_position_int_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_heartbeat_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_mount_status_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED,
+                                        mavlink_nav_controller_output_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_param_value_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED,
+                                        mavlink_remote_log_data_block_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_scaled_pressure_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_scaled_pressure2_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_servo_output_raw_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_sys_status_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_statustext_t &msg UNUSED)
+    {
+    }
+    virtual void handle_decoded_message(uint64_t T UNUSED, mavlink_vfr_hud_t &msg UNUSED)
+    {
+    }
+
+protected:
+    uint8_t system_id;
+    uint8_t component_id;
+
+private:
+};
+
+#endif

sololink/flightcode/dataflash_logger/mavlink_reader.cpp

@@ -0,0 +1,82 @@
+#include "mavlink_reader.h"
+
+#include <errno.h>
+#include <stdio.h>  // for perror
+#include <stdlib.h> // for abort
+
+#include <unistd.h> // for usleep
+
+#include <signal.h>
+#include <fcntl.h>
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include "analyzer_util.h"
+#include "la-log.h"
+
+/* This is used to prevent swamping the log with error messages if
+   something unexpected happens.
+   Returns -1 if we cannot log an error now, or returns the number of
+   messages skipped due to rate limiting if we can, i.e. a return of
+   2 means log, and we have skipped 2 messages due to rate limiting. */
+int MAVLink_Reader::can_log_error()
+{
+    unsigned ret_val;
+    uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
+    if ((now_us - err_time_us) < err_interval_us) {
+        /* can't log */
+        err_skipped++;
+        return -1;
+    }
+    /* yes; say we can and set err_time_us assuming we do log something */
+    err_time_us = now_us;
+    ret_val = err_skipped;
+    err_skipped = 0;
+    return ret_val;
+}
+
+void MAVLink_Reader::handle_message_received(uint64_t timestamp, mavlink_message_t msg)
+{
+    for (int i = 0; i < next_message_handler; i++) {
+        ((MAVLink_Message_Handler *)message_handler[i])->handle_message(timestamp, msg);
+    }
+}
+
+uint32_t MAVLink_Reader::feed(const uint8_t *buf, const uint32_t len)
+{
+    for (uint32_t i = 0; i < len; i++) {
+        // ::printf("Read (%d)\n", i);
+        if (is_tlog() && !done_timestamp) {
+            timestamp <<= 8;
+            timestamp |= (uint8_t)(buf[i]);
+            if (timestamp_offset++ == 7) {
+                done_timestamp = true;
+                // ::printf("timestamp (%lu)\n", timestamp);
+                timestamp_offset = 0;
+            }
+        } else {
+            if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &mav_msg, &mav_status)) {
+                if (!is_tlog()) {
+                    timestamp = now();
+                }
+                // printf("message received at %d (parse_errors=%u)\n", i, mav_status.parse_error);
+                handle_message_received(timestamp, mav_msg);
+                packet_count++;
+                done_timestamp = false;
+                timestamp = 0;
+            } else {
+                // printf("   %u state: %u\n", i, mav_status.parse_state);
+            }
+        }
+    }
+    return len; // at the moment we parse everything we receive
+}
+
+void MAVLink_Reader::end_of_log()
+{
+    for (int i = 0; i < next_message_handler; i++) {
+        message_handler[i]->end_of_log(packet_count);
+    }
+}

sololink/flightcode/dataflash_logger/mavlink_reader.h

@@ -0,0 +1,61 @@
+#ifndef MAVLINK_READER_H
+#define MAVLINK_READER_H
+
+#include <stdint.h>
+#include <netinet/in.h>
+
+#include "INIReader.h"
+#include "util.h"
+
+#include "mavlink_message_handler.h"
+
+#include "format_reader.h"
+
+/* A mavlink packet should be limited to 6+255+2 = 263 bytes
+   6 byte header, 255 max bytes in payload, 2 byte crc */
+#define TELEM_PKT_MAX 512
+
+class MAVLink_Reader : public Format_Reader
+{
+public:
+    MAVLink_Reader(INIReader *config)
+        : Format_Reader(config), err_skipped(0), err_time_us(0), /* last time we logged */
+          err_interval_us(1000000),                              /* once per second max */
+          _is_tlog(false)
+    {
+    }
+
+    uint32_t feed(const uint8_t *buf, const uint32_t len) override;
+
+    void set_is_tlog(bool value)
+    {
+        _is_tlog = value;
+    }
+    bool is_tlog()
+    {
+        return _is_tlog;
+    }
+
+protected:
+    void end_of_log() override;
+
+private:
+    int can_log_error();
+
+    void handle_message_received(uint64_t timestamp, mavlink_message_t msg);
+
+    uint16_t err_skipped;
+    uint64_t err_time_us;
+    uint64_t err_interval_us;
+
+    uint32_t packet_count = 0;
+    uint8_t timestamp_offset = 0;
+    bool done_timestamp = false;
+    uint64_t timestamp = 0;
+    mavlink_message_t mav_msg;
+    mavlink_status_t mav_status;
+
+    bool _is_tlog;
+};
+
+#endif

sololink/flightcode/dataflash_logger/mavlink_writer.cpp

@@ -0,0 +1,29 @@
+#include "mavlink_writer.h"
+
+#include <algorithm>
+
+void MAVLink_Writer::add_client(Telem_Client *client)
+{
+    clients.push_back(client);
+}
+
+void MAVLink_Writer::send_message(const mavlink_message_t &msg)
+{
+    // std::for_each(clients.begin(), clients.end(), send_message(msg));
+    std::for_each(clients.begin(), clients.end(), [msg](Telem_Client *c) { c->send_message(msg); });
+    // for (std::vector<Telem_Client *>::iterator it = clients.begin();
+    //      it != clients.end();
+    //      it++) {
+    //     (*it)->send_message(msg);
+    // }
+}
+
+bool MAVLink_Writer::any_data_to_send()
+{
+    for (std::vector< Telem_Client * >::iterator it = clients.begin(); it != clients.end(); it++) {
+        if ((*it)->any_data_to_send()) {
+            return true;
+        }
+    }
+    return false;
+}

sololink/flightcode/dataflash_logger/mavlink_writer.h

@@ -0,0 +1,27 @@
+#ifndef _MAVLINK_WRITER_H
+#define _MAVLINK_WRITER_H
+
+#include <vector>
+
+#include "../mavlink/c_library/ardupilotmega/mavlink.h"
+#include "INIReader.h"
+#include "telem_client.h"
+
+#define UNUSED __attribute__((unused))
+
+class MAVLink_Writer
+{
+public:
+    MAVLink_Writer(INIReader *config UNUSED)
+    {
+    }
+    void send_message(const mavlink_message_t &msg);
+
+    void add_client(Telem_Client *);
+    bool any_data_to_send();
+
+private:
+    std::vector< Telem_Client * > clients;
+};
+
+#endif

sololink/flightcode/dataflash_logger/message_handler.h

@@ -0,0 +1,39 @@
+#ifndef MESSAGE_HANDLER_H
+#define MESSAGE_HANDLER_H
+
+#define UNUSED __attribute__((unused))
+
+class Message_Handler
+{
+public:
+    friend class Format_Reader;
+
+    virtual bool configure(INIReader *config UNUSED)
+    {
+        return true;
+    }
+
+    virtual void sighup_received()
+    {
+    }
+
+    virtual void end_of_log(uint32_t packet_count UNUSED)
+    {
+    }
+
+protected:
+    virtual void idle_tenthHz()
+    {
+    }
+    virtual void idle_1Hz()
+    {
+    }
+    virtual void idle_10Hz()
+    {
+    }
+    virtual void idle_100Hz()
+    {
+    }
+};
+
+#endif

sololink/flightcode/dataflash_logger/telem_client.cpp

@@ -0,0 +1,13 @@
+#include "telem_client.h"
+
+uint32_t Telem_Client::send_buffer_used()
+{
+    if (_send_buf_stop >= _send_buf_start) {
+        return _send_buf_stop - _send_buf_start;
+    }
+    return send_buf_size() - _send_buf_start + _send_buf_stop;
+}
+uint32_t Telem_Client::send_buffer_space_free()
+{
+    return send_buf_size() - send_buffer_used();
+}

sololink/flightcode/dataflash_logger/telem_client.h

@@ -0,0 +1,44 @@
+#ifndef _TELEM_CLIENT_H
+#define _TELEM_CLIENT_H
+
+#include "../mavlink/c_library/ardupilotmega/mavlink.h"
+#include <sys/select.h>
+#include "INIReader.h"
+
+class Telem_Client
+{
+public:
+    Telem_Client(uint8_t *recv_buf, uint32_t recv_buflen)
+        : _recv_buf(recv_buf), _recv_buflen(recv_buflen)
+    {
+    }
+
+    virtual void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                 uint8_t &nfds) = 0;
+    virtual void configure(INIReader *config) = 0;
+    virtual void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err,
+                                   uint8_t &nfds) = 0;
+
+    virtual void init() = 0;
+
+    virtual void do_writer_sends() = 0;
+    virtual bool send_message(const mavlink_message_t &message) = 0;
+    virtual bool any_data_to_send() = 0;
+
+    virtual uint32_t send_buffer_space_free();
+    virtual uint32_t send_buffer_used();
+
+    uint32_t _send_buf_start = 0;
+    uint32_t _send_buf_stop = 0;
+
+    // FIXME: scope
+    uint8_t *_recv_buf;    // receive buffer
+    uint32_t _recv_buflen; // receive buffer len
+    uint32_t _recv_buflen_content = 0;
+
+    virtual uint32_t send_buf_size() const = 0;
+
+private:
+};
+
+#endif

sololink/flightcode/dataflash_logger/telem_forwarder_client.cpp

@@ -0,0 +1,174 @@
+#include "telem_forwarder_client.h"
+
+#include <string.h>
+// #include <sys/socket.h>
+// #include <netinet/in.h>
+// #include <arpa/inet.h>
+
+#include "la-log.h"
+
+#define UNUSED __attribute__((unused))
+
+void Telem_Forwarder_Client::pack_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED,
+                                             fd_set &fds_err, uint8_t &nfds)
+{
+    FD_SET(fd_telem_forwarder, &fds_read);
+    FD_SET(fd_telem_forwarder, &fds_err);
+
+    if (fd_telem_forwarder >= nfds) {
+        nfds = fd_telem_forwarder + 1;
+    }
+}
+
+void Telem_Forwarder_Client::handle_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED,
+                                               fd_set &fds_err, uint8_t &nfds UNUSED)
+{
+    /* check for packets from telem_forwarder */
+    if (FD_ISSET(fd_telem_forwarder, &fds_err)) {
+        FD_CLR(fd_telem_forwarder, &fds_err);
+        la_log(LOG_ERR, "select(fd_telem_forwarder): %s", strerror(errno));
+    }
+
+    if (FD_ISSET(fd_telem_forwarder, &fds_read)) {
+        FD_CLR(fd_telem_forwarder, &fds_read);
+        _recv_buflen_content = handle_recv();
+        // ::fprintf(stderr, "received %u bytes\n", _buflen_content);
+    }
+}
+
+/*
+* create_and_bind - create a socket and bind it to a local UDP port
+*
+* Used to create the socket on the upstream side that receives from and sends
+* to telem_forwarder
+*
+* Returns fd on success, -1 on error.
+*/
+void Telem_Forwarder_Client::create_and_bind()
+{
+    int fd;
+    struct sockaddr_in sa;
+
+    fd = socket(AF_INET, SOCK_DGRAM, 0);
+    if (fd < 0) {
+        perror("socket");
+        abort();
+    }
+
+    memset(&sa, 0, sizeof(sa));
+    sa.sin_family = AF_INET;
+    sa.sin_addr.s_addr = htonl(INADDR_ANY);
+    sa.sin_port = 0; // we don't care what our port is
+
+    if (bind(fd, (struct sockaddr *)&sa, sizeof(sa)) < 0) {
+        perror("bind");
+        abort();
+    }
+
+    fd_telem_forwarder = fd;
+} /* create_and_bind */
+
+void Telem_Forwarder_Client::pack_telem_forwarder_sockaddr(INIReader *config)
+{
+    // uint16_t tf_port = config->GetInteger("solo", "telem_forward_port", 14560);
+    // std::string ip = config->Get("solo", "soloIp", "10.1.1.10");
+    uint16_t tf_port = config->GetInteger("solo", "telem_forward_port", 14560);
+    std::string ip = config->Get("solo", "soloIp", "127.0.0.1");
+
+    la_log(LOG_INFO, "df-tfc: connecting to telem-forwarder at %s:%u", ip.c_str(), tf_port);
+    memset(&sa_tf, 0, sizeof(sa_tf));
+    sa_tf.sin_family = AF_INET;
+    //    sa_tf.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
+    inet_aton(ip.c_str(), &sa_tf.sin_addr); // useful for debugging
+    sa_tf.sin_port = htons(tf_port);
+}
+
+bool Telem_Forwarder_Client::sane_telem_forwarder_packet(uint8_t *pkt, uint16_t pktlen)
+{
+    if (sa.sin_addr.s_addr != sa_tf.sin_addr.s_addr) {
+        la_log(LOG_ERR, "received packet not from solo (0x%08x)", sa.sin_addr.s_addr);
+        return false;
+    }
+    if (pktlen < 8) {
+        la_log(LOG_ERR, "received runt packet (%d bytes)", pktlen);
+        return false;
+    }
+    if (pkt[0] != 254) {
+        la_log(LOG_ERR, "received bad magic (0x%02x)", pkt[0]);
+        return false;
+    }
+    if (pkt[1] != (pktlen - 8)) {
+        la_log(LOG_ERR, "inconsistent length (%u, %u)", pkt[1], pktlen);
+        return false;
+    }
+    return true;
+}
+
+uint32_t Telem_Forwarder_Client::handle_recv()
+{
+    // ::printf("Receiving packet into position %u\n", _buflen_content);
+    /* packet from telem_forwarder */
+    socklen_t sa_len = sizeof(sa);
+    uint16_t res =
+        recvfrom(fd_telem_forwarder, &_recv_buf[_recv_buflen_content],
+                 _recv_buflen - _recv_buflen_content, 0, (struct sockaddr *)&sa, &sa_len);
+
+    /* We get one mavlink packet per udp datagram. Sanity checks here
+       are: must be from solo's IP and have a valid mavlink header. */
+    // FIXME: we don't necessarily get just one packet/buffer!
+    // ::fprintf(stderr, "handle_recv\n");
+    if (!sane_telem_forwarder_packet(_recv_buf, res)) {
+        return 0;
+    }
+
+    return res;
+}
+
+bool Telem_Forwarder_Client::send_message(const mavlink_message_t &msg)
+{
+    if (send_buffer_space_free() < 1) {
+        // dropped_packets++;
+        return false;
+    }
+    memcpy(&_send_buf[_send_buf_stop++], (char *)&msg, sizeof(msg));
+    if (_send_buf_stop >= send_buf_size()) {
+        _send_buf_stop = 0;
+    }
+    return true;
+}
+
+void Telem_Forwarder_Client::do_writer_sends()
+{
+    char buf[1024]; // large enough...
+
+    while (_send_buf_start != _send_buf_stop) {
+        mavlink_message_t &msg = _send_buf[_send_buf_start];
+        uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg);
+
+        int32_t bytes_sent = sendto(fd_telem_forwarder, buf, messageLen, 0,
+                                    (struct sockaddr *)&sa_tf, sizeof(struct sockaddr));
+        if (bytes_sent == -1) {
+            la_log(LOG_INFO, "Failed sendto: %s", strerror(errno));
+            // we drop the message anyway!
+        }
+        _send_buf_start++;
+        if (_send_buf_start >= send_buf_size()) {
+            _send_buf_start = 0;
+        }
+    }
+
+    return;
+}
+
+void Telem_Forwarder_Client::configure(INIReader *config)
+{
+    /* prepare sockaddr used to contact telem_forwarder */
+    pack_telem_forwarder_sockaddr(config);
+}
+
+void Telem_Forwarder_Client::init()
+{
+    /* Prepare a port to receive and send data to/from telem_forwarder */
+    /* does not return on failure */
+    create_and_bind();
+}

sololink/flightcode/dataflash_logger/telem_forwarder_client.h

@@ -0,0 +1,47 @@
+#include "INIReader.h"
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#include "telem_client.h"
+
+class Telem_Forwarder_Client : public Telem_Client
+{
+public:
+    Telem_Forwarder_Client(uint8_t *recv_buf, uint32_t recv_buflen)
+        : Telem_Client(recv_buf, recv_buflen)
+    {
+    }
+
+    uint32_t handle_recv();
+    void init() override;
+
+    void configure(INIReader *config);
+    void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds);
+    void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds);
+
+    void do_writer_sends();
+    bool send_message(const mavlink_message_t &msg);
+    bool any_data_to_send()
+    {
+        return _send_buf_start != _send_buf_stop;
+    }
+
+private:
+    int fd_telem_forwarder = -1;
+
+    struct sockaddr_in sa;    // our send-from address
+    struct sockaddr_in sa_tf; /* telem_forwarder's address */
+
+    void create_and_bind();
+    void pack_telem_forwarder_sockaddr(INIReader *config);
+    bool sane_telem_forwarder_packet(uint8_t *pkt, uint16_t bpktlen);
+
+    /* send buffer stuff: */
+    mavlink_message_t _send_buf[256]; // way too bug?
+    uint32_t send_buf_size() const
+    {
+        return 256;
+    }
+};

sololink/flightcode/dataflash_logger/telem_serial.cpp

@@ -0,0 +1,217 @@
+#include "telem_serial.h"
+
+#include "la-log.h"
+
+#include <fcntl.h>
+#include <string.h>
+#include <termios.h>
+#include <unistd.h>
+
+#define UNUSED __attribute__((unused))
+
+void Telem_Serial::init()
+{
+    open_serial_port();
+}
+
+void Telem_Serial::pack_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, fd_set &fds_err,
+                                   uint8_t &nfds)
+{
+    FD_SET(fd, &fds_read);
+    FD_SET(fd, &fds_err);
+
+    if (fd >= nfds) {
+        nfds = fd + 1;
+    }
+}
+
+void Telem_Serial::handle_select_fds(fd_set &fds_read, fd_set &fds_write UNUSED, fd_set &fds_err,
+                                     uint8_t &nfds UNUSED)
+{
+    /* check for packets from telem_forwarder */
+    if (FD_ISSET(fd, &fds_err)) {
+        FD_CLR(fd, &fds_err);
+        la_log(LOG_ERR, "select(%d): %s", fd, strerror(errno));
+    }
+
+    if (FD_ISSET(fd, &fds_read)) {
+        FD_CLR(fd, &fds_read);
+        _recv_buflen_content = handle_read();
+        // ::fprintf(stderr, "received %u bytes\n", _buflen_content);
+    }
+}
+
+// from serial_setup in telem_forward
+void Telem_Serial::open_serial_port()
+{
+    fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+    if (fd < 0) {
+        la_log(LOG_ERR, "unable to open serial port %s", serialPortName.c_str());
+        abort();
+    }
+
+    // Configure port for 8N1 transmission
+    struct termios options;
+
+    tcgetattr(fd, &options); // Gets the current options for the port
+    // Set the output baud rate
+    switch (serialBaud) {
+    case 1200:
+        cfsetspeed(&options, B1200);
+        break;
+    case 2400:
+        cfsetspeed(&options, B2400);
+        break;
+    case 4800:
+        cfsetspeed(&options, B4800);
+        break;
+    case 9600:
+        cfsetspeed(&options, B9600);
+        break;
+    case 19200:
+        cfsetspeed(&options, B19200);
+        break;
+    case 38400:
+        cfsetspeed(&options, B38400);
+        break;
+    case 57600:
+        cfsetspeed(&options, B57600);
+        break;
+    case 115200:
+        cfsetspeed(&options, B115200);
+        break;
+    case 500000:
+        cfsetspeed(&options, B500000);
+        break;
+    case 921600:
+        cfsetspeed(&options, B921600);
+        break;
+    case 1500000:
+        cfsetspeed(&options, B1500000);
+        break;
+    default:
+        syslog(LOG_ERR, "unsupported baud rate %d", serialBaud);
+        return;
+    }
+    options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+    options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
+    options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+    options.c_cflag &= ~(CSIZE | PARENB);
+    options.c_cflag |= (CS8 | CLOCAL);
+
+    if (serialFlow) {
+        options.c_cflag |= CRTSCTS; // hardware flow control
+    } else {
+        options.c_cflag &= ~(CRTSCTS); // no hardware flow control
+    }
+
+    // At 115k (87 us per char), reading 1 char at a time results in increased
+    // CPU usage, since we actually can keep up with getting a small number of
+    // characters per loop. At 921k (11 us per char), we get more characters
+    // each time through the loop, so there is less advantage to setting VMIN
+    // to more than 1.
+    //
+    //          CPU Usage at
+    // VMIN     115k    921k
+    //    1     7.0%    1.8%
+    //   10     2.7%    1.6%
+    //  100     1.2%    1.2%
+    //
+    // The problem with asking for more than 1 character per read is that each
+    // message will usually not be received until some bytes in the following
+    // message are available. That is often not a problem, but there are
+    // sometimes gaps of several 10s of milliseconds in the telemetry stream,
+    // and it is preferable to process messages as soon as they are available.
+    if (serialBaud <= 115200) {
+        options.c_cc[VMIN] = 10;
+    } else {
+        options.c_cc[VMIN] = 1;
+    }
+    options.c_cc[VTIME] = 0;
+
+    tcsetattr(fd, TCSANOW, &options); // Set the new options for the port "NOW"
+
+    la_log(LOG_INFO, "opened serial port %s", serialPortName.c_str());
+
+    return;
+}
+
+uint32_t Telem_Serial::handle_read()
+{
+    ssize_t res = read(fd, _recv_buf, _recv_buflen);
+    if (res == -1) {
+        la_log(LOG_INFO, "Failed read: %s", strerror(errno));
+    }
+
+    return res;
+}
+
+void Telem_Serial::configure(INIReader *config)
+{
+    serialPortName = config->Get("solo", "telemDev", "/dev/ttymxc1");
+    serialBaud = config->GetInteger("solo", "telemBaud", 57600);
+    serialFlow = config->GetBoolean("solo", "telemFlow", true);
+}
+
+void Telem_Serial::do_writer_sends()
+{
+    while (_send_buf_start != _send_buf_stop) { // FIXME: use file descriptors!
+        bool tail_first = false;
+        if (_send_buf_stop < _send_buf_start) {
+            tail_first = true;
+        }
+        uint32_t bytes_to_send =
+            tail_first ? (send_buf_size() - _send_buf_start) : (_send_buf_stop - _send_buf_start);
+
+        int32_t sent = ::write(fd, (const char *)&_send_buf[_send_buf_start], bytes_to_send);
+        if (sent < 0) {
+            // cry
+            break;
+        } else if (sent == 0) {
+            break;
+        } else {
+            _send_buf_start += sent;
+            if (_send_buf_start == send_buf_size()) {
+                _send_buf_start = 0;
+            }
+        }
+    }
+}
+
+bool Telem_Serial::send_message(const mavlink_message_t &msg)
+{
+    char sendbuf[1024]; // large enough...
+
+    uint16_t messageLen = mavlink_msg_to_send_buffer((uint8_t *)sendbuf, &msg);
+    if (send_buffer_space_free() < messageLen) {
+        // dropped_packets++;
+        return false;
+    }
+    if (_send_buf_stop >= _send_buf_start) {
+        uint16_t to_copy = send_buf_size() - _send_buf_stop;
+        if (to_copy > messageLen) {
+            to_copy = messageLen;
+        }
+        memcpy(&_send_buf[_send_buf_stop], sendbuf, to_copy);
+        _send_buf_stop += to_copy;
+        if (_send_buf_stop >= send_buf_size()) {
+            _send_buf_stop = 0;
+        }
+        to_copy = messageLen - to_copy;
+        if (to_copy) {
+            memcpy(&_send_buf[_send_buf_stop], &sendbuf[messageLen - to_copy], to_copy);
+            _send_buf_stop += to_copy;
+            if (_send_buf_stop >= send_buf_size()) {
+                _send_buf_stop = 0;
+            }
+        }
+    } else {
+        memcpy(&_send_buf[_send_buf_stop], &sendbuf[0], messageLen);
+        _send_buf_stop += messageLen;
+        if (_send_buf_stop >= send_buf_size()) {
+            _send_buf_stop = 0;
+        }
+    }
+    return true;
+}

sololink/flightcode/dataflash_logger/telem_serial.h

@@ -0,0 +1,44 @@
+#include "INIReader.h"
+
+#include "telem_client.h"
+
+class Telem_Serial : public Telem_Client
+{
+public:
+    Telem_Serial(uint8_t *recv_buf, uint32_t recv_buflen) : Telem_Client(recv_buf, recv_buflen)
+    {
+    }
+
+    uint32_t handle_read();
+
+    void configure(INIReader *config);
+    void init() override;
+
+    void pack_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds);
+    void handle_select_fds(fd_set &fds_read, fd_set &fds_write, fd_set &fds_err, uint8_t &nfds);
+
+    bool send_message(const mavlink_message_t &message);
+
+    void open_serial_port();
+
+    void do_writer_sends();
+
+    bool any_data_to_send()
+    {
+        return _send_buf_start != _send_buf_stop;
+    }
+
+private:
+    int fd;
+
+    /* send buffer stuff: */
+    uint8_t _send_buf[65536]; /* way too big */
+    uint32_t send_buf_size() const
+    {
+        return 65536;
+    }
+
+    std::string serialPortName;
+    uint32_t serialBaud;
+    bool serialFlow;
+};

sololink/flightcode/dflog/Makefile

@@ -0,0 +1,45 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+VPATH = ../util
+
+INCS = -I../util
+
+CFLAGS += -Wall $(INCS) 
+CXXFLAGS += -Wall $(INCS)
+
+LIBS += -lpthread
+
+SRCS_CPP = dflog_downloader.cpp
+SRCS_C += util.c
+
+OBJS = $(SRCS_CPP:.cpp=.o) $(SRCS_C:.c=.o)
+
+MAIN = dflog
+
+all: $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(LINK.cpp) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+	$(RM) ../util/*.o
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/dflog/dataFlashMAVLink-to-artoo.py

@@ -0,0 +1,173 @@
+#!/usr/bin/python
+
+import subprocess
+import time
+import string
+import os
+import re
+
+actually_sync_logs_to_artoo = False
+
+class RecentLogLinker:
+    '''RecentLogLinker manages links to the most recent logs.  These links are used by the app to retrieve the most recent logs
+    '''
+    def __init__(self, directory,link_dir):
+        self.directory = directory #location of dataflashlogs
+        self.symlink_directory = link_dir #loction of links to the dataflashlogs
+        # if you reduce this number, you're going to have to ensure we
+        # remove all old stale RECENT links somehow!
+        self.num_recent_links_to_create = 10
+
+    # directly copied from LogPruner
+    def lognum(self, filename):
+        base = filename.partition('.')
+        return int(base[0])
+
+    # directly copied from LogPruner
+    def ordered_list_of_log_filepaths(self):
+        binfiles_regex = re.compile("[0-9]+.BIN")
+        files = [ f for f in os.listdir(self.directory) if (binfiles_regex.match(f) and os.path.isfile(os.path.join(self.directory, f))) ]
+        files.sort(key=self.lognum)
+        return [ os.path.join(self.directory, f)  for f in files ]
+
+    def update_links(self):
+        filepaths = self.ordered_list_of_log_filepaths()
+        #print("Filepaths: " + str(filepaths))
+        for i in range(0, self.num_recent_links_to_create):
+            if len(filepaths) == 0:
+                break
+            source = filepaths.pop()
+            if i == 0:
+                link_name = "RECENT.BIN"
+            else:
+                link_name = "RECENT-{0}.BIN".format(i)
+
+            link_path = os.path.join(self.symlink_directory, link_name)
+            if os.path.exists(link_path):
+                existing_source = None
+                try:
+                    existing_source = os.readlink(link_path)
+                except OSError as e:
+                    os.unlink(link_path)
+                if existing_source is not None:
+                    #print("Leaving link alone")
+                    if existing_source == source:
+                        continue
+                    os.unlink(link_path)
+
+            try:
+                os.symlink(source, link_path)
+            except OSError as e:
+                print "Failed to link ({0} to {1}): {2}".format(source, link_path,  e.strerror)
+                break
+
+class LogPruner:
+    '''LogPruner removes log files (e.g. 45.BIN) from the supplied
+    directory until only the suppied number of logs remain.  Files are
+    removed in ascending numerical order
+    '''
+    def __init__(self, directory, max_files_remaining):
+        self.directory = directory
+        self.max_files_remaining = max_files_remaining
+
+    # directly copied to RecentLogLinker
+    def lognum(self, filename):
+        base = filename.partition('.')
+        return int(base[0])
+
+    # directly copied to RecentLogLinker
+    def ordered_list_of_log_filepaths(self):
+        binfiles_regex = re.compile("[0-9]+.BIN")
+        files = [ f for f in os.listdir(self.directory) if (binfiles_regex.match(f) and os.path.isfile(os.path.join(self.directory, f))) ]
+        files.sort(key=self.lognum)
+        return [ os.path.join(self.directory, f)  for f in files ]
+
+    def run(self):
+        if not os.path.exists(self.directory):
+            print("Directory does not exist: {0}".format(self.directory))
+            return
+        ordered_list = self.ordered_list_of_log_filepaths()
+        while len(ordered_list) > self.max_files_remaining:
+            to_remove = ordered_list.pop(0)
+            try:
+                os.unlink(to_remove)
+            except IOError as e:
+                print "Failed to remove ({0}): {1}".format(to_remove, e.strerror)
+                break
+
+class LogSyncer:
+    '''
+    an instance of LogSyncer loops over rsync'ing dataflash logs from
+    the log data directory on solo onto Artoo
+    '''
+    def __init__(self, src, dest, ssh_id_path, rate=100):
+        self.rsyncSrc = src
+        self.rsyncDest = dest
+        self.ssh_id_path = ssh_id_path
+        self.rate = rate
+        self.rsyncPartialDest = '.'.join((self.rsyncDest,'part'))
+        if not os.path.exists(src):
+            os.makedirs(src)
+
+    def run(self):
+        # quick sanity checks here.  --delete is a very, very big hammer
+        if len(self.rsyncSrc) < 10:
+            print("short rsync src")
+            sys.exit(1)
+        if len(self.rsyncDest) < 10:
+            print("short rsync dest")
+            sys.exit(1)
+
+        recentloglinker = RecentLogLinker(src)
+
+	while True:
+            recentloglinker.update_links()
+
+            cmd = [ 'ionice', '-c', '3', 'nice',
+                "rsync", "-a", "--size-only", "--progress",
+                '-e', 'ssh -o StrictHostKeyChecking=no -i "%s"' % self.ssh_id_path,
+                "--partial",
+                "--delete",
+                '--bwlimit=%s' % (self.rate,),
+                self.rsyncSrc, self.rsyncDest]
+#            print "cmd: %s" % (' '.join(cmd))
+            p = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE)
+            stdout,stderr = p.communicate()
+            rc = p.returncode
+            if rc != 0:
+                print "stderr: (%s)" % stderr
+                time.sleep(1);
+                continue
+
+#            print "stdout: (%s)" % stdout
+
+            timeout = 10
+            if string.find(stdout,"xfr") != -1:
+                timeout = 1
+            time.sleep(timeout);
+
+# TODO: get artoo's address from config
+# TODO: get source and dest addresses from config
+src = "/log/dataflash/"
+dest = "root@10.1.1.1:/log/solo/dataflash"
+link_dir = "/log/" #location of symlinks for app to retrieve
+ssh_id_path = "/home/root/.ssh/id_rsa-mav-df-xfer"
+max_log_files=50
+
+p = LogPruner(src, max_log_files)
+p.run()
+
+if actually_sync_logs_to_artoo:
+    l = LogSyncer(src, dest, ssh_id_path)
+    l.run()
+else:
+    # we still update the links on SoloLink so the app can fetch them
+    recentloglinker = RecentLogLinker(src,link_dir)
+
+    while True:
+        recentloglinker.update_links()
+        time.sleep(10);
+
+# neither reached nor necessary if the LogSyncer runs
+while True:
+    time.sleep(86400)

sololink/flightcode/dflog/dflog_downloader.cpp

@@ -0,0 +1,497 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <iostream>
+#include <iomanip>
+#include <stdint.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <netdb.h>
+#include <time.h>
+#include <termios.h>
+#include <sys/errno.h>
+#include <sys/ioctl.h>
+#include <list>
+#include <sys/mman.h>
+#include <fstream>
+#include <poll.h>
+#include "util.h"
+#include "../mavlink/c_library/ardupilotmega/mavlink.h"
+#include "../mavlink/c_library/common/mavlink.h"
+#include <sstream>
+#include <signal.h>
+#include <pthread.h>
+#include <semaphore.h>
+#include <dirent.h>
+
+using namespace std;
+
+/***********************************************************************
+Serial port name
+***********************************************************************/
+string serialPortName;
+
+/***********************************************************************
+File descriptors
+***********************************************************************/
+int serial_fd;
+
+/***********************************************************************
+Threading variables
+***********************************************************************/
+pthread_mutex_t mutex_msg;
+pthread_mutex_t mutex_msgtype;
+mavlink_message_t latest_msg;
+sem_t sem_msg;
+int msg_type = 0;
+
+/***********************************************************************
+Log file variables
+***********************************************************************/
+int log_size;
+ofstream logfile;
+int log_num = 0;
+
+/***********************************************************************
+User input variables
+***********************************************************************/
+bool get_latest = false;
+
+/***********************************************************************
+Serial tx/rx buffer size
+***********************************************************************/
+#define BUFSIZE 256
+
+/***********************************************************************
+Pointer to log buffer.  Allocate a really big buffer later
+that is the size of the log file.  This is probably not the best way
+to go about this, but it works in a pinch.
+***********************************************************************/
+char *logbuf;
+
+/***********************************************************************
+Function: int serial_setup(int baud)
+
+Description: The serial port initialization function.  This function
+             initializes the serial port over which DSM data is sent to
+             the pixhawk.  A return of 0 indicates an error.
+***********************************************************************/
+int serial_setup(int baud)
+{
+    struct termios options;
+
+    serial_fd = open(serialPortName.c_str(), O_RDWR | O_NOCTTY);
+
+    if (serial_fd < 0) {
+        cerr << "Unable to open serial port " << serialPortName.c_str() << endl;
+        return 0;
+    }
+
+    tcflush(serial_fd, TCIOFLUSH);
+
+    // Configure port for 8N1 transmission
+    tcgetattr(serial_fd, &options); // Gets the current options for the port
+    // Set the output baud rate
+    switch (baud) {
+    case 57600:
+        cfsetspeed(&options, B57600);
+        break;
+    case 115200:
+        cfsetspeed(&options, B115200);
+        break;
+    case 1500000:
+        cfsetspeed(&options, B1500000);
+        break;
+    default:
+        cerr << "Unsupported baud rate" << endl;
+        return 0;
+    }
+    options.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
+    options.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
+    options.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG);
+    options.c_cflag &= ~(CSIZE | PARENB);
+    options.c_cflag |= CS8;
+    options.c_cflag |= CRTSCTS;
+    options.c_cc[VMIN] = 17;
+    options.c_cc[VTIME] = 0;
+
+    tcsetattr(serial_fd, TCSANOW, &options); // Set the new options for the port "NOW"
+
+    sleep(1);
+    tcflush(serial_fd, TCIOFLUSH);
+
+    cout << "Opened serial port " << serialPortName.c_str() << endl;
+
+    return 1;
+}
+
+/***********************************************************************
+Function: int print_pct()
+
+Description: Prints a pct bar on the console and displays the
+             current download speed.
+***********************************************************************/
+void print_pct(float pct, float speed)
+{
+    int maxWidth = 50;
+    int pos;
+    int i;
+
+    cout << "[";
+    pos = maxWidth * pct;
+    for (i = 0; i < maxWidth; ++i) {
+        if (i < pos)
+            cout << "=";
+        else if (i == pos)
+            cout << ">";
+        else
+            cout << " ";
+    }
+    cout << "] " << int(pct * 100.0) << "%";
+    cout << " " << speed << " kB/s  \r";
+    cout << flush;
+}
+
+/***********************************************************************
+Function: int handle_log_data()
+
+Description: Take log data and dump it into a log file
+***********************************************************************/
+int handle_log_data(mavlink_message_t *msg)
+{
+    int ofs, cnt, id;
+    uint64_t now_us;
+    float speed = 0.0;
+    char logdat[128];
+
+    static int fileofs = 0;
+    static int disc_cnt = 0;
+    static uint64_t last_us = 0;
+    static int bytes_received = 0;
+    static int last_bytes_received = 0;
+    static char *buf_ptr = logbuf;
+
+    id = mavlink_msg_log_data_get_id(msg);
+
+    if (id != log_num) {
+        cout << "Got data from a different log, ignoring" << endl;
+        return 0;
+    }
+
+    ofs = mavlink_msg_log_data_get_ofs(msg);
+    cnt = mavlink_msg_log_data_get_count(msg);
+    mavlink_msg_log_data_get_data(msg, (uint8_t *)logdat);
+
+    // I don't think this ever happens, but if we get a zero
+    // count then we're done
+    if (cnt == 0) {
+        cout << endl;
+        cout << "Log download complete" << endl;
+        return 1;
+    }
+
+    // Write data directly to the (really big) buffer
+    memcpy(buf_ptr, logdat, cnt);
+    buf_ptr += cnt;
+
+    if (fileofs != ofs)
+        disc_cnt++;
+
+    now_us = clock_gettime_us(CLOCK_MONOTONIC);
+
+    bytes_received += cnt;
+
+    // Output
+    if ((now_us - last_us) > 1e6) {
+        speed =
+            float(bytes_received - last_bytes_received) / 1024. / float((now_us - last_us) / 1e6);
+
+        print_pct(float(bytes_received) / float(log_size), speed);
+
+        last_us = now_us;
+        last_bytes_received = bytes_received;
+    }
+
+    // Update the file position
+    fileofs = ofs + cnt;
+
+    // Are we done yet?
+    if (fileofs == log_size) {
+        print_pct(1, speed);
+        cout << endl;
+        cout << "Log download complete." << endl;
+        cout << "Downloaded " << float(bytes_received) / 1024. << "kB, dropped " << disc_cnt
+             << " packet(s)" << endl;
+        return 1;
+    }
+
+    return 0;
+}
+
+/***********************************************************************
+Function: int mavlink_task()
+
+Description: Thread task to pull mavlink data from the serial port
+             and dump it into a mutex-protected mavlink message.
+
+             When log data begins streaming, dumps log data into the
+             log file on the filesystem.
+***********************************************************************/
+void *mavlink_task(void *)
+{
+    mavlink_message_t _msg;
+    mavlink_status_t mavlink_status;
+    int read_len;
+    int i;
+    char buf[BUFSIZE];
+    int msgtolookfor;
+
+    while (1) {
+        read_len = read(serial_fd, buf, BUFSIZE);
+
+        if (read_len < 0) {
+            if (errno != EAGAIN)
+                cerr << "Read err: " << errno << endl;
+        }
+
+        if (read_len > 0) {
+            for (i = 0; i < read_len; ++i) {
+                if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &_msg, &mavlink_status)) {
+                    // If this is the type of message we're waiting for, put it on
+                    // the latest_msg or handle its log data.
+                    pthread_mutex_lock(&mutex_msgtype);
+                    msgtolookfor = msg_type;
+                    pthread_mutex_unlock(&mutex_msgtype);
+
+                    if (_msg.msgid == msgtolookfor) {
+                        if (_msg.msgid == MAVLINK_MSG_ID_LOG_DATA) {
+                            if (handle_log_data(&_msg)) {
+                                cout << "Writing to logfile...";
+                                logfile.write(logbuf, log_size);
+                                system("sync");
+                                cout << "Done!" << endl;
+                                pthread_exit(NULL);
+                            }
+                        } else {
+                            pthread_mutex_lock(&mutex_msg);
+                            memcpy(&latest_msg, &_msg, sizeof(mavlink_message_t));
+                            pthread_mutex_unlock(&mutex_msg);
+                            sem_post(&sem_msg);
+                        }
+                    }
+                }
+            }
+        }
+    }
+    pthread_exit(NULL);
+}
+
+/***********************************************************************
+Function: int wait_for_message()
+
+Description: Blocks until a particular mavlink message type is received.
+             A return value of 0 indicates succes, -1 for a timeout.
+             Setting timeout_s to 0 blocks indefinitely.
+***********************************************************************/
+int wait_for_message(int _msg_type, mavlink_message_t *msg, int timeout_s)
+{
+    struct timespec timeout;
+    int ret;
+
+    // Tell the mavlink thread which message to look for
+    pthread_mutex_lock(&mutex_msgtype);
+    msg_type = _msg_type;
+    pthread_mutex_unlock(&mutex_msgtype);
+
+    clock_gettime(CLOCK_REALTIME, &timeout);
+    timeout.tv_sec += timeout_s;
+
+    if (timeout_s == 0)
+        sem_wait(&sem_msg);
+    else {
+        ret = sem_timedwait(&sem_msg, &timeout);
+
+        if (ret < 0) {
+            if (errno == ETIMEDOUT)
+                return -1;
+        }
+    }
+
+    pthread_mutex_lock(&mutex_msg);
+    memcpy(msg, &latest_msg, sizeof(mavlink_message_t));
+    pthread_mutex_unlock(&mutex_msg);
+
+    return 0;
+}
+
+/***********************************************************************
+Function: void request_log()
+
+Description: Request a particular log file from the pixhawk.
+***********************************************************************/
+void request_log(void)
+{
+    char buf[BUFSIZE];
+    mavlink_message_t msg;
+    int msg_len;
+    char logfilename[] = "/log/log12345.bin";
+
+    // Wait for a heartbeat
+    if (wait_for_message(MAVLINK_MSG_ID_HEARTBEAT, &msg, 5) < 0) {
+        cout << "No heartbeat received" << endl;
+        exit(1);
+    }
+
+    // Get the latest log number (num_logs works, last_log_num doesn't)
+    if (get_latest) {
+        mavlink_msg_log_request_list_pack(1, 1, &msg, 1, 1, log_num, log_num);
+        msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg);
+
+        if (write(serial_fd, buf, msg_len) != msg_len) {
+            cerr << "Serial port write error." << endl;
+            exit(1);
+        }
+
+        cout << "Requesting latest log number...";
+
+        if (wait_for_message(MAVLINK_MSG_ID_LOG_ENTRY, &msg, 10) < 0) {
+            cout << "Unable to get log information" << endl;
+            exit(1);
+        }
+
+        log_num = mavlink_msg_log_entry_get_num_logs(&msg);
+        cout << "Latest log is " << log_num << endl;
+    }
+
+    // Get some size information about the log we want.
+    mavlink_msg_log_request_list_pack(1, 1, &msg, 1, 1, log_num, log_num);
+    msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg);
+    if (write(serial_fd, buf, msg_len) != msg_len) {
+        cerr << "Serial port write error." << endl;
+        exit(1);
+    }
+
+    cout << "Waiting for log information..." << endl;
+
+    if (wait_for_message(MAVLINK_MSG_ID_LOG_ENTRY, &msg, 10) < 0) {
+        cout << "Unable to get log information" << endl;
+        exit(1);
+    }
+
+    log_size = mavlink_msg_log_entry_get_size(&msg);
+    if (log_size == 0) {
+        cout << "Log " << log_num << " does not exist." << endl;
+        exit(1);
+    } else {
+        cout << "Log size is " << float(log_size) / 1024. << "kB" << endl;
+
+        logbuf = new char[log_size];
+        if (logbuf == NULL) {
+            cout << "Unable to allocate buffer for log" << endl;
+            exit(1);
+        }
+    }
+
+    // Set up the log file storage
+    sprintf(logfilename, "/log/log%i.bin", log_num);
+    logfile.open(logfilename, ios::out | ios::binary);
+
+    // Request log data
+    cout << "Beginning log download..." << endl;
+    mavlink_msg_log_request_data_pack(1, 1, &msg, 1, 1, log_num, 0, 0xFFFFFFFF);
+    msg_len = mavlink_msg_to_send_buffer((uint8_t *)buf, &msg);
+
+    // Let the thread know we've requested data.
+    pthread_mutex_lock(&mutex_msgtype);
+    msg_type = MAVLINK_MSG_ID_LOG_DATA;
+    pthread_mutex_unlock(&mutex_msgtype);
+
+    if (write(serial_fd, buf, msg_len) != msg_len) {
+        cerr << "Serial port write error." << endl;
+        exit(1);
+    }
+
+    // Let the mavlink thread do its thing
+}
+
+/**********************************************************************
+Function: int main(void)
+
+Description: The main function.  Initializes and runs the serial and
+             UDP threads.
+***********************************************************************/
+int main(int argc, char *argv[])
+{
+    int baudrate;
+    pthread_t mavlink_ctx;
+    char usbdev_string[256] = "usb-3D_Robotics_PX4_FMU";
+    DIR *dir;
+    struct dirent *direntry;
+    bool foundDevice = false;
+
+    if (argc < 2) {
+        cout << "Usage: dflog <lognum|latest>" << endl;
+        return -1;
+    }
+
+    if (strcmp(argv[1], "latest") == 0) {
+        cout << "Pulling latest log" << endl;
+        get_latest = true;
+    } else {
+        log_num = atoi(argv[1]);
+        cout << "Pulling log " << log_num << endl;
+    }
+
+    // Cout cleanup
+    std::cout.precision(2);
+    std::cout.setf(ios::fixed, ios::floatfield);
+
+    dir = opendir("/dev/serial/by-id/");
+    if (dir == NULL) {
+        cerr << "open /dev/serial/by-id failed" << endl;
+        return -1;
+    }
+
+    while ((direntry = readdir(dir))) {
+        if (!strncmp(direntry->d_name, usbdev_string, 23)) {
+            foundDevice = true;
+            strcpy(usbdev_string, direntry->d_name);
+            break;
+        }
+    }
+
+    if (!foundDevice) {
+        cerr << "Unable to find USB device" << endl;
+        return -1;
+    }
+
+    closedir(dir);
+
+    // Serial port setup
+    serialPortName = "/dev/serial/by-id/";
+    serialPortName.append(usbdev_string);
+    baudrate = 115200;
+    if (!serial_setup(baudrate)) {
+        cerr << "Unable to initialize the serial send" << endl;
+        return -1;
+    }
+
+    // Threading initialization
+    pthread_mutex_init(&mutex_msg, NULL);
+    sem_init(&sem_msg, 0, 0);
+
+    // Start the mavlink rx thread
+    pthread_create(&mavlink_ctx, NULL, mavlink_task, NULL);
+
+    // Send a log request to the pixhawk
+    request_log();
+
+    // Wait for the mavlink thread to end
+    pthread_join(mavlink_ctx, NULL);
+
+    logfile.close();
+
+    return 0;
+}

sololink/flightcode/dflog/loadLog.py

@@ -0,0 +1,82 @@
+#!/usr/bin/env python
+
+import subprocess 
+import sys
+import os
+import time
+from pymavlink import mavutil
+import glob
+import ConfigParser
+import shutil
+from datetime import datetime
+import argparse
+
+SELECT_GPIO = "21"
+ENABLE_GPIO = "19"
+
+#GPIO direction set
+def setGPIODir(gpio, direction):
+    dir_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/direction", "w")
+    dir_fd.write(direction)
+    dir_fd.close()
+
+#Open the GPIO
+def openGPIO(gpio):
+    #Check and see if the GPIO is already exported
+    if not os.path.isdir("/sys/class/gpio/gpio"+str(gpio)):
+        #otherwise export it
+        exp_fd = open("/sys/class/gpio/export", "w")
+        exp_fd.write(gpio)
+        exp_fd.close()
+
+    setGPIODir(gpio, "out");
+
+def closeGPIO(gpio):
+    unexp_fd = open("/sys/class/gpio/unexport", "w")
+    unexp_fd.write(gpio)
+    unexp_fd.close()
+
+def setGPIO(gpio, value):
+    val_fd = open("/sys/class/gpio/gpio"+str(gpio)+"/value", "w")
+    val_fd.write(value)
+    val_fd.close()
+
+def openSetClose(gpio, value):
+    openGPIO(gpio)
+    setGPIO(gpio, value)
+    closeGPIO(gpio)
+
+#Set the GPIO low
+def disconnectAndExit():
+    openSetClose(SELECT_GPIO, "0")
+    openSetClose(ENABLE_GPIO, "1")
+    sys.exit()
+
+parser = argparse.ArgumentParser()
+parser.add_argument("lognum", help="Log number to download, or 'latest'")
+args = parser.parse_args()
+
+#Log downloading process
+print "Pixhawk log loader"
+
+#Set the USB select GPIOs
+openSetClose(SELECT_GPIO, "1")
+openSetClose(ENABLE_GPIO, "0")
+time.sleep(1)
+
+print "Checking for pixhawk on USB"
+
+usb_devs = glob.glob('/dev/serial/by-id/usb-3D*')
+if not usb_devs:
+    print "No pixhawk found on USB.  Exiting."
+    disconnectAndExit()
+    
+print "Pixhawk found on USB, requesting log."
+pixhawk_usb = usb_devs[-1]
+m = mavutil.mavlink_connection(pixhawk_usb)
+
+#Call the log downloader app
+ret = subprocess.call(["dflog", str(args.lognum)])
+
+disconnectAndExit()
+

sololink/flightcode/hostapd_ctrl/Makefile

@@ -0,0 +1,40 @@
+# To cross compile:
+#
+# Set up as usual for bitbake:
+# $ . setup-environment build
+#
+# In the build directory:
+# $ bitbake meta-ide-support
+# $ . tmp/environment-setup-cortexa9hf-vfp-neon-poky-linux-gnueabi
+#
+# Now a make in this directory should work.
+
+VPATH = ../util
+
+INCS = -I../util
+
+CFLAGS += -Wall $(INCS)
+
+SRCS_C = main.c hostapd_ctrl.c util.c
+
+OBJS = $(SRCS_C:.c=.o)
+
+MAIN = hostapd_ctrl
+
+all: $(MAIN)
+
+$(MAIN): $(OBJS)
+	$(LINK.c) -o $(MAIN) $(OBJS) $(LIBS)
+
+clean:
+	$(RM) *.o *~ $(MAIN)
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/hostapd_ctrl/main.c

@@ -0,0 +1,48 @@
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include "util.h"
+#include "hostapd_ctrl.h"
+
+/* Test program for hostapd_ctrl */
+
+/* make this small (e.g. 2) to test the table-too-small case */
+#define STATIONS_MAX 10
+
+int main(int argc, char *argv[])
+{
+    void *handle;
+    hostapd_station_info_t station_info[STATIONS_MAX];
+    int stations;
+    int i;
+    char mac_string[MAC_STRING_LEN];
+
+    handle = hostapd_ctrl_new("wlan0-ap");
+    if (handle == NULL) {
+        printf("ERROR creating control connection\n");
+        exit(1);
+    }
+
+    stations = STATIONS_MAX;
+    if (hostapd_ctrl_get_stations(handle, station_info, &stations) != 0) {
+        printf("ERROR getting station info\n");
+        hostapd_ctrl_delete(handle);
+        exit(1);
+    }
+
+    printf("%d stations:\n", stations);
+
+    if (stations > STATIONS_MAX)
+        /* there are more stations than would fit in the table we supplied */
+        stations = STATIONS_MAX;
+
+    for (i = 0; i < stations; i++)
+        printf("%s\n", mac_ntoa(station_info[i].mac, mac_string));
+
+    hostapd_ctrl_delete(handle);
+
+    exit(0);
+
+} /* main */

sololink/flightcode/ini/Makefile

@@ -0,0 +1,14 @@
+
+all:
+
+clean:
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/ini/cpp/INIReader.cpp

@@ -0,0 +1,75 @@
+// Read an INI file into easy-to-access name/value pairs.
+
+#include <algorithm>
+#include <cctype>
+#include <cstdlib>
+#include "../ini.h"
+#include "INIReader.h"
+
+using std::string;
+
+INIReader::INIReader(string filename)
+{
+    _error = ini_parse(filename.c_str(), ValueHandler, this);
+}
+
+int INIReader::ParseError()
+{
+    return _error;
+}
+
+string INIReader::Get(string section, string name, string default_value)
+{
+    string key = MakeKey(section, name);
+    return _values.count(key) ? _values[key] : default_value;
+}
+
+long INIReader::GetInteger(string section, string name, long default_value)
+{
+    string valstr = Get(section, name, "");
+    const char *value = valstr.c_str();
+    char *end;
+    // This parses "1234" (decimal) and also "0x4D2" (hex)
+    long n = strtol(value, &end, 0);
+    return end > value ? n : default_value;
+}
+
+double INIReader::GetReal(string section, string name, double default_value)
+{
+    string valstr = Get(section, name, "");
+    const char *value = valstr.c_str();
+    char *end;
+    double n = strtod(value, &end);
+    return end > value ? n : default_value;
+}
+
+bool INIReader::GetBoolean(string section, string name, bool default_value)
+{
+    string valstr = Get(section, name, "");
+    // Convert to lower case to make string comparisons case-insensitive
+    std::transform(valstr.begin(), valstr.end(), valstr.begin(), ::tolower);
+    if (valstr == "true" || valstr == "yes" || valstr == "on" || valstr == "1")
+        return true;
+    else if (valstr == "false" || valstr == "no" || valstr == "off" || valstr == "0")
+        return false;
+    else
+        return default_value;
+}
+
+string INIReader::MakeKey(string section, string name)
+{
+    string key = section + "." + name;
+    // Convert to lower case to make section/name lookups case-insensitive
+    std::transform(key.begin(), key.end(), key.begin(), ::tolower);
+    return key;
+}
+
+int INIReader::ValueHandler(void *user, const char *section, const char *name, const char *value)
+{
+    INIReader *reader = (INIReader *)user;
+    string key = MakeKey(section, name);
+    if (reader->_values[key].size() > 0)
+        reader->_values[key] += "\n";
+    reader->_values[key] += value;
+    return 1;
+}

sololink/flightcode/ini/cpp/INIReader.h

@@ -0,0 +1,51 @@
+// Read an INI file into easy-to-access name/value pairs.
+
+// inih and INIReader are released under the New BSD license (see LICENSE.txt).
+// Go to the project home page for more info:
+//
+// http://code.google.com/p/inih/
+
+#ifndef __INIREADER_H__
+#define __INIREADER_H__
+
+#include <map>
+#include <string>
+
+// Read an INI file into easy-to-access name/value pairs. (Note that I've gone
+// for simplicity here rather than speed, but it should be pretty decent.)
+class INIReader
+{
+public:
+    // Construct INIReader and parse given filename. See ini.h for more info
+    // about the parsing.
+    INIReader(std::string filename);
+
+    // Return the result of ini_parse(), i.e., 0 on success, line number of
+    // first error on parse error, or -1 on file open error.
+    int ParseError();
+
+    // Get a string value from INI file, returning default_value if not found.
+    std::string Get(std::string section, std::string name, std::string default_value);
+
+    // Get an integer (long) value from INI file, returning default_value if
+    // not found or not a valid integer (decimal "1234", "-1234", or hex "0x4d2").
+    long GetInteger(std::string section, std::string name, long default_value);
+
+    // Get a real (floating point double) value from INI file, returning
+    // default_value if not found or not a valid floating point value
+    // according to strtod().
+    double GetReal(std::string section, std::string name, double default_value);
+
+    // Get a boolean value from INI file, returning default_value if not found or if
+    // not a valid true/false value. Valid true values are "true", "yes", "on", "1",
+    // and valid false values are "false", "no", "off", "0" (not case sensitive).
+    bool GetBoolean(std::string section, std::string name, bool default_value);
+
+private:
+    int _error;
+    std::map< std::string, std::string > _values;
+    static std::string MakeKey(std::string section, std::string name);
+    static int ValueHandler(void *user, const char *section, const char *name, const char *value);
+};
+
+#endif // __INIREADER_H__

sololink/flightcode/ini/cpp/Makefile

@@ -0,0 +1,14 @@
+
+all:
+
+clean:
+
+BASE := ../../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/ini/ini.c

@@ -0,0 +1,174 @@
+/* inih -- simple .INI file parser
+
+inih is released under the New BSD license (see LICENSE.txt). Go to the project
+home page for more info:
+
+http://code.google.com/p/inih/
+
+*/
+
+#include <stdio.h>
+#include <ctype.h>
+#include <string.h>
+
+#include "ini.h"
+
+#if !INI_USE_STACK
+#include <stdlib.h>
+#endif
+
+#define MAX_SECTION 50
+#define MAX_NAME 50
+
+/* Strip whitespace chars off end of given string, in place. Return s. */
+static char *rstrip(char *s)
+{
+    char *p = s + strlen(s);
+    while (p > s && isspace((unsigned char)(*--p)))
+        *p = '\0';
+    return s;
+}
+
+/* Return pointer to first non-whitespace char in given string. */
+static char *lskip(const char *s)
+{
+    while (*s && isspace((unsigned char)(*s)))
+        s++;
+    return (char *)s;
+}
+
+/* Return pointer to first char c or ';' comment in given string, or pointer to
+   null at end of string if neither found. ';' must be prefixed by a whitespace
+   character to register as a comment. */
+static char *find_char_or_comment(const char *s, char c)
+{
+    int was_whitespace = 0;
+    while (*s && *s != c && !(was_whitespace && *s == ';')) {
+        was_whitespace = isspace((unsigned char)(*s));
+        s++;
+    }
+    return (char *)s;
+}
+
+/* Version of strncpy that ensures dest (size bytes) is null-terminated. */
+static char *strncpy0(char *dest, const char *src, size_t size)
+{
+    strncpy(dest, src, size);
+    dest[size - 1] = '\0';
+    return dest;
+}
+
+/* See documentation in header file. */
+int ini_parse_file(FILE *file, int (*handler)(void *, const char *, const char *, const char *),
+                   void *user)
+{
+/* Uses a fair bit of stack (use heap instead if you need to) */
+#if INI_USE_STACK
+    char line[INI_MAX_LINE];
+#else
+    char *line;
+#endif
+    char section[MAX_SECTION] = "";
+    char prev_name[MAX_NAME] = "";
+
+    char *start;
+    char *end;
+    char *name;
+    char *value;
+    int lineno = 0;
+    int error = 0;
+
+#if !INI_USE_STACK
+    line = (char *)malloc(INI_MAX_LINE);
+    if (!line) {
+        return -2;
+    }
+#endif
+
+    /* Scan through file line by line */
+    while (fgets(line, INI_MAX_LINE, file) != NULL) {
+        lineno++;
+
+        start = line;
+#if INI_ALLOW_BOM
+        if (lineno == 1 && (unsigned char)start[0] == 0xEF && (unsigned char)start[1] == 0xBB &&
+            (unsigned char)start[2] == 0xBF) {
+            start += 3;
+        }
+#endif
+        start = lskip(rstrip(start));
+
+        if (*start == ';' || *start == '#') {
+            /* Per Python ConfigParser, allow '#' comments at start of line */
+        }
+#if INI_ALLOW_MULTILINE
+        else if (*prev_name && *start && start > line) {
+            /* Non-black line with leading whitespace, treat as continuation
+               of previous name's value (as per Python ConfigParser). */
+            if (!handler(user, section, prev_name, start) && !error)
+                error = lineno;
+        }
+#endif
+        else if (*start == '[') {
+            /* A "[section]" line */
+            end = find_char_or_comment(start + 1, ']');
+            if (*end == ']') {
+                *end = '\0';
+                strncpy0(section, start + 1, sizeof(section));
+                *prev_name = '\0';
+            } else if (!error) {
+                /* No ']' found on section line */
+                error = lineno;
+            }
+        } else if (*start && *start != ';') {
+            /* Not a comment, must be a name[=:]value pair */
+            end = find_char_or_comment(start, '=');
+            if (*end != '=') {
+                end = find_char_or_comment(start, ':');
+            }
+            if (*end == '=' || *end == ':') {
+                *end = '\0';
+                name = rstrip(start);
+                value = lskip(end + 1);
+                end = find_char_or_comment(value, '\0');
+                if (*end == ';')
+                    *end = '\0';
+                rstrip(value);
+
+                /* Valid name[=:]value pair found, call handler */
+                strncpy0(prev_name, name, sizeof(prev_name));
+                if (!handler(user, section, name, value) && !error)
+                    error = lineno;
+            } else if (!error) {
+                /* No '=' or ':' found on name[=:]value line */
+                error = lineno;
+            }
+        }
+
+#if INI_STOP_ON_FIRST_ERROR
+        if (error)
+            break;
+#endif
+    }
+
+#if !INI_USE_STACK
+    free(line);
+#endif
+
+    return error;
+}
+
+/* See documentation in header file. */
+int ini_parse(const char *filename,
+              int (*handler)(void *, const char *, const char *, const char *), void *user)
+{
+    FILE *file;
+    int error;
+
+    file = fopen(filename, "r");
+    if (!file)
+        return -1;
+    error = ini_parse_file(file, handler, user);
+    fclose(file);
+    return error;
+}

sololink/flightcode/ini/ini.h

@@ -0,0 +1,75 @@
+/* inih -- simple .INI file parser
+
+inih is released under the New BSD license (see LICENSE.txt). Go to the project
+home page for more info:
+
+http://code.google.com/p/inih/
+
+*/
+
+#ifndef __INI_H__
+#define __INI_H__
+
+/* Make this header file easier to include in C++ code */
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+
+/* Parse given INI-style file. May have [section]s, name=value pairs
+   (whitespace stripped), and comments starting with ';' (semicolon). Section
+   is "" if name=value pair parsed before any section heading. name:value
+   pairs are also supported as a concession to Python's ConfigParser.
+
+   For each name=value pair parsed, call handler function with given user
+   pointer as well as section, name, and value (data only valid for duration
+   of handler call). Handler should return nonzero on success, zero on error.
+
+   Returns 0 on success, line number of first error on parse error (doesn't
+   stop on first error), -1 on file open error, or -2 on memory allocation
+   error (only when INI_USE_STACK is zero).
+*/
+int ini_parse(const char *filename,
+              int (*handler)(void *user, const char *section, const char *name, const char *value),
+              void *user);
+
+/* Same as ini_parse(), but takes a FILE* instead of filename. This doesn't
+   close the file when it's finished -- the caller must do that. */
+int ini_parse_file(FILE *file, int (*handler)(void *user, const char *section, const char *name,
+                                              const char *value),
+                   void *user);
+
+/* Nonzero to allow multi-line value parsing, in the style of Python's
+   ConfigParser. If allowed, ini_parse() will call the handler with the same
+   name for each subsequent line parsed. */
+#ifndef INI_ALLOW_MULTILINE
+#define INI_ALLOW_MULTILINE 1
+#endif
+
+/* Nonzero to allow a UTF-8 BOM sequence (0xEF 0xBB 0xBF) at the start of
+   the file. See http://code.google.com/p/inih/issues/detail?id=21 */
+#ifndef INI_ALLOW_BOM
+#define INI_ALLOW_BOM 1
+#endif
+
+/* Nonzero to use stack, zero to use heap (malloc/free). */
+#ifndef INI_USE_STACK
+#define INI_USE_STACK 1
+#endif
+
+/* Stop parsing on first error (default is to keep parsing). */
+#ifndef INI_STOP_ON_FIRST_ERROR
+#define INI_STOP_ON_FIRST_ERROR 0
+#endif
+
+/* Maximum line length for any line in INI file. */
+#ifndef INI_MAX_LINE
+#define INI_MAX_LINE 200
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __INI_H__ */

sololink/flightcode/log/Log.cpp

@@ -0,0 +1,160 @@
+#include <iostream>
+#include <stdio.h>
+#include <string.h>
+#include <sys/stat.h>
+#include <sstream>
+#include <fstream>
+#include "util.h"
+#include "Log.h"
+
+using namespace std;
+
+/**********************************************************************
+Function: Log()
+
+Description: The log constructor.  Opens an fstream file based on
+             the filename string.  Sets the maximum size of the log file
+             and the maximum number of log files to be written.
+***********************************************************************/
+Log::Log(string filename, long int maxFileSize, int maxLogFiles)
+    : _maxFileSize(maxFileSize), _maxLogFiles(maxLogFiles), _logFilename(filename)
+{
+    log_fd.precision(2);
+    log_fd.setf(ios::fixed, ios::floatfield);
+
+    // Open the log file
+    log_fd.open(filename.c_str(), ios::out | std::ofstream::app);
+    if (!log_fd) {
+        cerr << "Could not open log file" << endl;
+        return;
+    }
+}
+
+/**********************************************************************
+Function: Log()
+
+Description: Overloaded log constructor with an option for creating a
+             new log when the log starts up
+***********************************************************************/
+Log::Log(string filename, long int maxFileSize, int maxLogFiles, bool newLogOnBoot)
+    : _maxFileSize(maxFileSize), _maxLogFiles(maxLogFiles), _logFilename(filename)
+{
+    log_fd.precision(2);
+    log_fd.setf(ios::fixed, ios::floatfield);
+    bool fileExists = doesFileExist(filename);
+
+    // Open the log file
+    log_fd.open(filename.c_str(), ios::out | std::ofstream::app);
+    if (!log_fd) {
+        cerr << "Could not open log file" << endl;
+        return;
+    }
+
+    if (newLogOnBoot && fileExists)
+        forceRoll();
+}
+
+/**********************************************************************
+Function: getFilesize()
+
+Description: Gets the filesize of a file named by filename
+***********************************************************************/
+long int Log::getFilesize(string filename)
+{
+    struct stat filestatus;
+
+    stat(filename.c_str(), &filestatus);
+
+    return filestatus.st_size;
+}
+
+/**********************************************************************
+Function: doesFileExist()
+
+Description: Returns a bool indicating if the file named filename
+             exists.  filename must be an absolute path.
+***********************************************************************/
+bool Log::doesFileExist(string filename)
+{
+    struct stat filestatus;
+
+    return (stat(filename.c_str(), &filestatus) == 0);
+}
+
+/***********************************************************************
+Function: char* getTimeString(void)
+
+Description: Gets the current time string.
+ ***********************************************************************/
+const char *Log::getTimeString(void)
+{
+    static char buffer[80];
+
+    clock_gettime_str_r(CLOCK_REALTIME, buffer);
+
+    return buffer;
+}
+
+/**********************************************************************
+Function: checkSizeAndRoll()
+
+Description:  Checks to see if the log file has grown too big and should
+              be rolled over.  Rolls it and all previous logfiles
+              if necessary.  The calling application should call this
+              function periodically after writing to the logfile.
+***********************************************************************/
+void Log::checkSizeAndRoll(void)
+{
+    stringstream filename, newfilename;
+
+    // Check the size of the file.  If its too big, roll the logfiles.
+    if (getFilesize(_logFilename) > _maxFileSize) {
+        // Roll the log files up to the max-1
+        for (int i = 1; i < _maxLogFiles; ++i) {
+            filename.str(string());
+            filename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i);
+
+            // See if the file exists
+            if (doesFileExist(filename.str())) {
+                newfilename.str(string());
+                newfilename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i + 1);
+
+                rename(filename.str().c_str(), newfilename.str().c_str());
+            }
+        }
+
+        // Roll the current file
+        log_fd.close();
+        rename(_logFilename.c_str(), filename.str().c_str());
+        log_fd.open(_logFilename.c_str(), ios::out | std::ofstream::app);
+    }
+}
+
+/**********************************************************************
+Function: forceRoll()
+
+Description:  Forces a file roll
+***********************************************************************/
+void Log::forceRoll(void)
+{
+    stringstream filename, newfilename;
+
+    // Roll the log files up to the max-1
+    for (int i = 1; i < _maxLogFiles; ++i) {
+        filename.str(string());
+        filename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i);
+
+        // See if the file exists
+        if (doesFileExist(filename.str())) {
+            newfilename.str(string());
+            newfilename << _logFilename.c_str() << "." << dec << (int)(_maxLogFiles - i + 1);
+
+            rename(filename.str().c_str(), newfilename.str().c_str());
+        }
+    }
+
+    // Roll the current file
+    log_fd.close();
+    rename(_logFilename.c_str(), filename.str().c_str());
+    log_fd.open(_logFilename.c_str(), ios::out | std::ofstream::app);
+}

sololink/flightcode/log/Log.h

@@ -0,0 +1,41 @@
+#ifndef _LOG_H
+#define _LOG_H
+
+#include <stdio.h>
+#include <string.h>
+#include <fstream>
+
+using namespace std;
+
+/**********************************************************************
+Class: Log
+
+Description: A simple logging class.  Opens a logfile specified by the
+             calling function.  The log file can be rolled when its
+             size gets large enough, out to maxLogFiles files.
+***********************************************************************/
+class Log
+{
+public:
+    Log(string filename, long int maxFileSize, int maxLogFiles);
+    Log(string filename, long int maxFileSize, int maxLogFiles, bool newLogOnBoot);
+    void checkSizeAndRoll(void);
+    void forceRoll(void);
+
+    // Left as a public for use in logging macros
+    const char *getTimeString(void);
+
+    // Leave the file descriptor public so it can be written to
+    // by the calling application.
+    ofstream log_fd;
+
+private:
+    long int _maxFileSize;
+    int _maxLogFiles;
+    string _logFilename;
+
+    bool doesFileExist(string filename);
+    long int getFilesize(string filename);
+};
+
+#endif //_LOG_H

sololink/flightcode/log/Makefile

@@ -0,0 +1,14 @@
+
+all:
+
+clean:
+
+BASE := ../..
+
+fmt:
+	@python $(BASE)/tools/build/clang-format-run.py --apply
+
+fmt-diff:
+	@python $(BASE)/tools/build/clang-format-run.py
+
+.PHONY: all clean fmt fmt-diff

sololink/flightcode/mavlink/c_library/ardupilotmega/mavlink.h

@@ -0,0 +1,27 @@
+/** @file
+ *	@brief MAVLink comm protocol built from ardupilotmega.xml
+ *	@see http://mavlink.org
+ */
+#ifndef MAVLINK_H
+#define MAVLINK_H
+
+#ifndef MAVLINK_STX
+#define MAVLINK_STX 254
+#endif
+
+#ifndef MAVLINK_ENDIAN
+#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN
+#endif
+
+#ifndef MAVLINK_ALIGNED_FIELDS
+#define MAVLINK_ALIGNED_FIELDS 1
+#endif
+
+#ifndef MAVLINK_CRC_EXTRA
+#define MAVLINK_CRC_EXTRA 1
+#endif
+
+#include "version.h"
+#include "ardupilotmega.h"
+
+#endif // MAVLINK_H

sololink/flightcode/mavlink/c_library/ardupilotmega/mavlink_msg_ahrs.h

@@ -0,0 +1,353 @@
+// MESSAGE AHRS PACKING
+
+#define MAVLINK_MSG_ID_AHRS 163
+
+typedef struct __mavlink_ahrs_t
+{
+ float omegaIx; ///< X gyro drift estimate rad/s
+ float omegaIy; ///< Y gyro drift estimate rad/s
+ float omegaIz; ///< Z gyro drift estimate rad/s
+ float accel_weight; ///< average accel_weight
+ float renorm_val; ///< average renormalisation value
+ float error_rp; ///< average error_roll_pitch value
+ float error_yaw; ///< average error_yaw value
+} mavlink_ahrs_t;
+
+#define MAVLINK_MSG_ID_AHRS_LEN 28
+#define MAVLINK_MSG_ID_163_LEN 28
+
+#define MAVLINK_MSG_ID_AHRS_CRC 127
+#define MAVLINK_MSG_ID_163_CRC 127
+
+
+
+#define MAVLINK_MESSAGE_INFO_AHRS { \
+	"AHRS", \
+	7, \
+	{  { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \
+         { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \
+         { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \
+         { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \
+         { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \
+         { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \
+         { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \
+         } \
+}
+
+
+/**
+ * @brief Pack a ahrs 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 omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
+						       float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_AHRS_LEN];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+}
+
+/**
+ * @brief Pack a ahrs message on a channel
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
+							   mavlink_message_t* msg,
+						           float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_AHRS_LEN];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AHRS_LEN);
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+
+	msg->msgid = MAVLINK_MSG_ID_AHRS;
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+}
+
+/**
+ * @brief Encode a ahrs struct
+ *
+ * @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 ahrs C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
+{
+	return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
+}
+
+/**
+ * @brief Encode a ahrs struct on a channel
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param chan The MAVLink channel this message will be sent over
+ * @param msg The MAVLink message to compress the data into
+ * @param ahrs C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_ahrs_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs)
+{
+	return mavlink_msg_ahrs_pack_chan(system_id, component_id, chan, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw);
+}
+
+/**
+ * @brief Send a ahrs message
+ * @param chan MAVLink channel to send the message
+ *
+ * @param omegaIx X gyro drift estimate rad/s
+ * @param omegaIy Y gyro drift estimate rad/s
+ * @param omegaIz Z gyro drift estimate rad/s
+ * @param accel_weight average accel_weight
+ * @param renorm_val average renormalisation value
+ * @param error_rp average error_roll_pitch value
+ * @param error_yaw average error_yaw value
+ */
+#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
+
+static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char buf[MAVLINK_MSG_ID_AHRS_LEN];
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#else
+	mavlink_ahrs_t packet;
+	packet.omegaIx = omegaIx;
+	packet.omegaIy = omegaIy;
+	packet.omegaIz = omegaIz;
+	packet.accel_weight = accel_weight;
+	packet.renorm_val = renorm_val;
+	packet.error_rp = error_rp;
+	packet.error_yaw = error_yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#endif
+}
+
+#if MAVLINK_MSG_ID_AHRS_LEN <= MAVLINK_MAX_PAYLOAD_LEN
+/*
+  This varient of _send() can be used to save stack space by re-using
+  memory from the receive buffer.  The caller provides a
+  mavlink_message_t which is the size of a full mavlink message. This
+  is usually the receive buffer for the channel, and allows a reply to an
+  incoming message with minimum stack space usage.
+ */
+static inline void mavlink_msg_ahrs_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan,  float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw)
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+	char *buf = (char *)msgbuf;
+	_mav_put_float(buf, 0, omegaIx);
+	_mav_put_float(buf, 4, omegaIy);
+	_mav_put_float(buf, 8, omegaIz);
+	_mav_put_float(buf, 12, accel_weight);
+	_mav_put_float(buf, 16, renorm_val);
+	_mav_put_float(buf, 20, error_rp);
+	_mav_put_float(buf, 24, error_yaw);
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#else
+	mavlink_ahrs_t *packet = (mavlink_ahrs_t *)msgbuf;
+	packet->omegaIx = omegaIx;
+	packet->omegaIy = omegaIy;
+	packet->omegaIz = omegaIz;
+	packet->accel_weight = accel_weight;
+	packet->renorm_val = renorm_val;
+	packet->error_rp = error_rp;
+	packet->error_yaw = error_yaw;
+
+#if MAVLINK_CRC_EXTRA
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN, MAVLINK_MSG_ID_AHRS_CRC);
+#else
+    _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)packet, MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+#endif
+}
+#endif
+
+#endif
+
+// MESSAGE AHRS UNPACKING
+
+
+/**
+ * @brief Get field omegaIx from ahrs message
+ *
+ * @return X gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  0);
+}
+
+/**
+ * @brief Get field omegaIy from ahrs message
+ *
+ * @return Y gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  4);
+}
+
+/**
+ * @brief Get field omegaIz from ahrs message
+ *
+ * @return Z gyro drift estimate rad/s
+ */
+static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  8);
+}
+
+/**
+ * @brief Get field accel_weight from ahrs message
+ *
+ * @return average accel_weight
+ */
+static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  12);
+}
+
+/**
+ * @brief Get field renorm_val from ahrs message
+ *
+ * @return average renormalisation value
+ */
+static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  16);
+}
+
+/**
+ * @brief Get field error_rp from ahrs message
+ *
+ * @return average error_roll_pitch value
+ */
+static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  20);
+}
+
+/**
+ * @brief Get field error_yaw from ahrs message
+ *
+ * @return average error_yaw value
+ */
+static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg)
+{
+	return _MAV_RETURN_float(msg,  24);
+}
+
+/**
+ * @brief Decode a ahrs message into a struct
+ *
+ * @param msg The message to decode
+ * @param ahrs C-struct to decode the message contents into
+ */
+static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs)
+{
+#if MAVLINK_NEED_BYTE_SWAP
+	ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg);
+	ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg);
+	ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg);
+	ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg);
+	ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg);
+	ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg);
+	ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg);
+#else
+	memcpy(ahrs, _MAV_PAYLOAD(msg), MAVLINK_MSG_ID_AHRS_LEN);
+#endif
+}