travis: switch to pip for fetching the genmsg dependencies.

Ubuntu Precise doesn't have a pre-packaged version of catkin.

Signed-off-by: Michael Hope <mlhx@google.com>

.travis.yml

@@ -29,7 +29,11 @@ install:
   - sudo apt-get install build-essential git libc6-i386
   - tar -xf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2
   - export PATH=$PATH:$PWD/gcc-arm-none-eabi-4_8-2014q3/bin
-   
+  - sudo apt-get install -qq python-pip
+  - sudo pip install -q empy catkin_pkg
+  - git clone https://github.com/ros/genmsg
+  - cd genmsg && sudo python setup.py install && cd ..
+
 before_script: arm-none-eabi-gcc --version
 script: ./.travis.sh
 
@@ -38,4 +42,4 @@ cache: apt
 notifications:
   irc: "chat.freenode.net#cleanflight"
   use_notice: true
-  skip_join: true
+  skip_join: true

Makefile

@@ -61,7 +61,9 @@ REVISION = $(shell git log -1 --format="%h")
 # Working directories
 ROOT		 := $(patsubst %/,%,$(dir $(lastword $(MAKEFILE_LIST))))
 SRC_DIR		 = $(ROOT)/src/main
+MSG_DIR		 = $(ROOT)/msg
 OBJECT_DIR	 = $(ROOT)/obj/main
+GEN_DIR	 = $(ROOT)/obj/gen
 BIN_DIR		 = $(ROOT)/obj
 CMSIS_DIR	 = $(ROOT)/lib/main/CMSIS
 INCLUDE_DIRS	 = $(SRC_DIR)
@@ -207,7 +209,8 @@ TARGET_DIR = $(ROOT)/src/main/target/NAZE
 endif
 
 INCLUDE_DIRS := $(INCLUDE_DIRS) \
-		    $(TARGET_DIR)
+		$(TARGET_DIR) \
+		$(GEN_DIR)
 
 VPATH		:= $(VPATH):$(TARGET_DIR)
 
@@ -532,6 +535,8 @@ SPRACINGF3_SRC	 = \
 		   $(HIGHEND_SRC) \
 		   $(COMMON_SRC)
 
+MSG_SRC 	 = $(wildcard $(MSG_DIR)/*.msg)
+
 # Search path and source files for the ST stdperiph library
 VPATH		:= $(VPATH):$(STDPERIPH_DIR)/src
 
@@ -543,6 +548,7 @@ VPATH		:= $(VPATH):$(STDPERIPH_DIR)/src
 CC		 = arm-none-eabi-gcc
 OBJCOPY		 = arm-none-eabi-objcopy
 SIZE		 = arm-none-eabi-size
+GENMSP		 = python $(ROOT)/support/genmsp/genmsp.py
 
 #
 # Tool options.
@@ -610,6 +616,7 @@ TARGET_ELF	 = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).elf
 TARGET_OBJS	 = $(addsuffix .o,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC))))
 TARGET_DEPS	 = $(addsuffix .d,$(addprefix $(OBJECT_DIR)/$(TARGET)/,$(basename $($(TARGET)_SRC))))
 TARGET_MAP	 = $(OBJECT_DIR)/$(FORKNAME)_$(TARGET).map
+MSG_GEN		 = $(MSG_SRC:$(MSG_DIR)/%.msg=$(GEN_DIR)/msg/%.h)
 
 # List of buildable ELF files and their object dependencies.
 # It would be nice to compute these lists, but that seems to be just beyond make.
@@ -641,6 +648,10 @@ $(OBJECT_DIR)/$(TARGET)/%.o: %.S
 	@echo %% $(notdir $<)
 	@$(CC) -c -o $@ $(ASFLAGS) $<
 
+$(GEN_DIR)/msg/%.h: $(MSG_DIR)/%.msg $(SRC_DIR)/io/msg.h.template
+	@echo %% genmsp $(notdir $<)
+	@$(GENMSP) -p clearflight -o $(@D) -e $(SRC_DIR)/io -I clearflight:$(ROOT)/msg $<
+
 clean:
 	rm -f $(TARGET_BIN) $(TARGET_HEX) $(TARGET_ELF) $(TARGET_OBJS) $(TARGET_MAP)
 	rm -rf $(OBJECT_DIR)/$(TARGET)
@@ -675,3 +686,5 @@ $(TARGET_OBJS) : Makefile
 
 # include auto-generated dependencies
 -include $(TARGET_DEPS)
+
+$(OBJECT_DIR)/$(TARGET)/io/serial_msp.o: $(MSG_GEN)

msg/ApiVersion.msg

@@ -0,0 +1,3 @@
+uint8 protocol
+uint8 major
+uint8 minor

msg/BoardInfo.msg

@@ -0,0 +1,2 @@
+char[4] identifier
+uint16 hardwareRevision

msg/BuildInfo.msg

@@ -0,0 +1,3 @@
+char[11] date
+char[8] time
+char[7] shortGitRevision

msg/FcVariant.msg

@@ -0,0 +1 @@
+char[4] identifier

msg/FcVersion.msg

@@ -0,0 +1,3 @@
+uint8 major
+uint8 minor
+uint8 patchLevel

msg/ServoConf.msg

@@ -0,0 +1 @@
+ServoParam[10] servo

msg/ServoParam.msg

@@ -0,0 +1,6 @@
+string clearflight_type=auto
+
+int16 min
+int16 max
+int16 middle
+int8 rate

src/main/io/msg.h.template

@@ -0,0 +1,40 @@
+@# MSP encoder generator for Clearflight.
+@# EmPy format.  See http://www.alcyone.com/software/empy/ for more.
+@#
+// Generated by gencpp from file @(spec.package)/@(spec.short_name).msg
+// DO NOT EDIT!
+
+@{
+import genmsp
+env = genmsp.make_env(spec, msg_context, search_path)
+}@
+
+@# Generate the serializer for a message.
+@[def serialize(spec, prefix='', indent='')]@
+@[for field in spec.parsed_fields()]@
+@{ctype = genmsp.get_type(field.type)}@
+@[if ctype is None]@
+@{child = genmsp.load_spec(field.base_type, spec, env)}@
+@[if field.is_array]@
+    for (int i = 0; i < @genmsp.get_count(field); i++) {
+@serialize(child, '{}[i].'.format(field.name), ' '*4)@
+    }
+@[else]@
+@serialize(child, '{}->'.format(field.name))@
+@[end if]@
+@[else]@
+@[if field.is_array]@
+    @(indent)serialize@(ctype.encoder)s((const uint@(ctype.encoder)_t *)@prefix@field.name, @genmsp.get_count(field));
+@[else]@
+    @(indent)serialize@(ctype.encoder)(@prefix@field.name);
+@[end if]@
+@[end if]@
+@[end for]@
+@[end def]@
+
+// Respond to a @spec.short_name / MSP_@genmsp.to_caps(spec.short_name) command.
+static void send@(spec.short_name)(@(', '.join(genmsp.generate_encoder_args(spec, env)))) {
+    uint8_t len = @(' + '.join(str(genmsp.get_length(x, spec, env)) for x in spec.parsed_fields()));
+    headSerialReply(len);
+@serialize(spec)
+}

src/main/io/serial_msp.c

@@ -430,6 +430,13 @@ static void serialize8(uint8_t a)
     currentPort->checksum ^= a;
 }
 
+static void serialize8s(const uint8_t *p, int n)
+{
+    for (const uint8_t *pend = p + n; p != pend; p++) {
+        serialize8(*p);
+    }
+}
+
 static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
@@ -699,6 +706,15 @@ void mspInit(serialConfig_t *serialConfig)
 
 #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
 
+#include "msg/ApiVersion.h"
+#include "msg/FcVariant.h"
+#include "msg/FcVersion.h"
+#include "msg/BoardInfo.h"
+#include "msg/BuildInfo.h"
+#ifdef USE_SERVOS
+#include "msg/ServoConf.h"
+#endif
+
 static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
@@ -710,64 +726,33 @@ static bool processOutCommand(uint8_t cmdMSP)
 
     switch (cmdMSP) {
     case MSP_API_VERSION:
-        headSerialReply(
-            1 + // protocol version length
-            API_VERSION_LENGTH
-        );
-        serialize8(MSP_PROTOCOL_VERSION);
-
-        serialize8(API_VERSION_MAJOR);
-        serialize8(API_VERSION_MINOR);
+        sendApiVersion(MSP_PROTOCOL_VERSION,
+                       API_VERSION_MAJOR,
+                       API_VERSION_MINOR);
         break;
 
     case MSP_FC_VARIANT:
-        headSerialReply(FLIGHT_CONTROLLER_IDENTIFIER_LENGTH);
-
-        for (i = 0; i < FLIGHT_CONTROLLER_IDENTIFIER_LENGTH; i++) {
-            serialize8(flightControllerIdentifier[i]);
-        }
+        sendFcVariant(flightControllerIdentifier);
         break;
 
     case MSP_FC_VERSION:
-        headSerialReply(FLIGHT_CONTROLLER_VERSION_LENGTH);
-
-        serialize8(FC_VERSION_MAJOR);
-        serialize8(FC_VERSION_MINOR);
-        serialize8(FC_VERSION_PATCH_LEVEL);
+        sendFcVersion(FC_VERSION_MAJOR,
+                      FC_VERSION_MINOR,
+                      FC_VERSION_PATCH_LEVEL);
         break;
 
     case MSP_BOARD_INFO:
-        headSerialReply(
-            BOARD_IDENTIFIER_LENGTH +
-            BOARD_HARDWARE_REVISION_LENGTH
-        );
-        for (i = 0; i < BOARD_IDENTIFIER_LENGTH; i++) {
-            serialize8(boardIdentifier[i]);
-        }
+        sendBoardInfo(boardIdentifier,
 #ifdef NAZE
-        serialize16(hardwareRevision);
+                      hardwareRevision
 #else
-        serialize16(0); // No other build targets currently have hardware revision detection.
+                      0 // No other build targets currently have hardware revision detection.
 #endif
+            );
         break;
 
     case MSP_BUILD_INFO:
-        headSerialReply(
-                BUILD_DATE_LENGTH +
-                BUILD_TIME_LENGTH +
-                GIT_SHORT_REVISION_LENGTH
-        );
-
-        for (i = 0; i < BUILD_DATE_LENGTH; i++) {
-            serialize8(buildDate[i]);
-        }
-        for (i = 0; i < BUILD_TIME_LENGTH; i++) {
-            serialize8(buildTime[i]);
-        }
-
-        for (i = 0; i < GIT_SHORT_REVISION_LENGTH; i++) {
-            serialize8(shortGitRevision[i]);
-        }
+        sendBuildInfo(buildDate, buildTime, shortGitRevision);
         break;
 
     // DEPRECATED - Use MSP_API_VERSION
@@ -842,13 +827,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         s_struct((uint8_t *)&servo, 16);
         break;
     case MSP_SERVO_CONF:
-        headSerialReply(MAX_SUPPORTED_SERVOS * 7);
-        for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
-            serialize16(currentProfile->servoConf[i].min);
-            serialize16(currentProfile->servoConf[i].max);
-            serialize16(currentProfile->servoConf[i].middle);
-            serialize8(currentProfile->servoConf[i].rate);
-        }
+        sendServoConf(currentProfile->servoConf);
         break;
     case MSP_CHANNEL_FORWARDING:
         headSerialReply(MAX_SUPPORTED_SERVOS);

support/genmsp/README.md

@@ -0,0 +1,30 @@
+genmsp
+======
+Converts ROS messages into the Cleanflight native encoders and
+decoders.
+
+See:
+
+* msg/ for the messages.
+* http://wiki.ros.org/msg for the message format.
+
+genmsp depends on the [genmsg](https://github.com/ros/genmsg).
+See http://www.ros.org/install/ for pre-built Ubuntu packages
+including `ros-jade-genmsg` which installs under
+`/opt/ros/jade/lib/python2.7/dist-packages`.
+
+Implementation
+--------------
+The conversion is designed to be backwards compatible and to minimise
+the size of the flight software.  To do this, it has certain features
+and limitations:
+
+* Fixed length arrays show as `const Foo *field`.
+* Variable length arrays show as `const Foo *field` and `int nfield`
+  pair.
+* Values are passed as arguments.  As such, messages can't be nested
+  past one level deep.
+* To reduce copying or duplication, inner messages can be aliased to
+  the existing native form using `string clearflight_type=fooBar_t'.
+
+-- Michael Hope <mlhx@google.com> <michaelh@juju.net.nz>

support/genmsp/genmsp.py

@@ -0,0 +1,154 @@
+"""Convert ROS format messages to Clearflight format encoders."""
+
+import collections
+import re
+import sys
+
+import genmsg.msgs
+import genmsg.template_tools
+
+Env = collections.namedtuple('Env', 'spec msg_context search_path')
+
+# Details on the native version of a ROS type.
+Type = collections.namedtuple('Type', 'name size encoder')
+
+# All base types.
+MSG_TYPES = {
+    'char': Type('char', 1, '8'),
+    'bool': Type('bool', 1, '8'),
+    'uint8': Type('uint8_t', 1, '8'),
+    'int8': Type('int8_t', 1, '8'),
+    'uint16': Type('uint16_t', 2, '16'),
+    'int16': Type('int16_t', 2, '16'),
+    'uint32': Type('uint32_t', 4, '32'),
+    'int32': Type('int32_t', 4, '32'),
+    'uint64': Type('uint64_t', 8, '64'),
+    'int64': Type('int64_t', 8, '64'),
+    'float32': Type('float', 4, 'Float'),
+    'float64': Type('double', 8, 'Double'),
+    'string': Type('const char *', None, 'String'),
+    'time': Type('ros::Time', 4, '32'),
+    'duration': Type('ros::Duration', 4, '32'),
+}
+
+
+def make_env(spec, msg_context, search_path):
+    """Wrap commonly used arguments up into an environment."""
+    return Env(spec, msg_context, search_path)
+
+
+def load_spec(typename, spec, env):
+    """Find and load the spec of the given message type."""
+    t = genmsg.msgs.resolve_type(typename, spec.package)
+    assert isinstance(env.search_path, dict)
+    return genmsg.msg_loader.load_msg_by_type(
+        env.msg_context, t, env.search_path)
+
+
+def get_type(typename):
+    """Gets the type of a basic type.
+
+    Returns None for other types.
+
+    """
+    base_type, _, _ = genmsg.msgs.parse_type(typename)
+    return MSG_TYPES.get(base_type, None)
+
+
+def to_caps(name):
+    """Convers a CamelCase string to CAPS_CASE."""
+    return re.sub(r'([a-z])([A-Z])', r'\1_\2', name).upper()
+
+
+def _multiply(left, right):
+    """Returns a pretty string version of multiplying the arguments."""
+    if left == 1:
+        return right
+    else:
+        return '{}*{}'.format(left, right)
+
+
+def _lcfirst(name):
+    """Converts the first letter of the string to lower case."""
+    return name[0].lower() + name[1:]
+
+
+def get_length(field, spec, env):
+    """Returns the byte length of a field as a string."""
+    ctype = get_type(field.type)
+    if ctype is not None:
+        # A base type.
+        if field.base_type == 'string':
+            if get_count(field) != 1:
+                raise RuntimeError(
+                    ('{}: support for arrays of strings is not '
+                     'implemented'.format(field.name)))
+            return 'strlen({})'.format(field.name)
+        else:
+            return _multiply(get_count(field), ctype.size)
+    else:
+        # Load the message and return the size of it.
+        child = load_spec(field.base_type, spec, env)
+        sizes = (str(get_length(x, child, env)) for x in child.parsed_fields())
+        joined = '({})'.format(' + '.join(sizes))
+        return _multiply(get_count(field), joined)
+
+
+def get_count(field):
+    """Returns the number of items in a field."""
+    if not field.is_array:
+        return 1
+    if field.array_len is not None:
+        return field.array_len
+    # Return the name of the parameter that holds the length.
+    return 'n{}'.format(field.name)
+
+
+def _msg_type_to_cpp(name, spec, env):
+    base_type, is_array, _ = genmsg.msgs.parse_type(name)
+    ctype = get_type(base_type)
+    if ctype:
+        if is_array:
+            return 'const {} *'.format(ctype.name)
+        else:
+            return ctype.name
+    else:
+        if '/' in base_type:
+            # Use the short type.
+            base_type = base_type.split('/')[-1]
+        # Check if the message overrides the native type name.
+        child = load_spec(base_type, spec, env)
+        constants = {x.name: x.val for x in child.constants}
+        ctype = constants.get('clearflight_type', base_type)
+        if ctype == 'auto':
+            # Remap messages named FooBar to fooBar_t
+            ctype = _lcfirst(base_type) + '_t'
+        return 'const {} *'.format(ctype)
+
+
+def generate_encoder_args(spec, env):
+    """Generates the function arguments for each field."""
+    for field in spec.parsed_fields():
+        ctype = get_type(field.base_type)
+        cname = _msg_type_to_cpp(field.base_type, spec, env)
+        if ctype is None:
+            yield '{}{}'.format(cname, field.name)
+            if field.is_array and field.array_len is None:
+                yield 'int n{}'.format(field.name)
+        else:
+            if field.is_array:
+                yield 'const {} *{}'.format(cname, field.name)
+                if field.array_len is None:
+                    yield 'int n{}'.format(field.name)
+            else:
+                yield '{} {}'.format(cname, field.name)
+
+
+def main():
+    msg_template_map = {'msg.h.template': '@NAME@.h'}
+
+    genmsg.template_tools.generate_from_command_line_options(
+        sys.argv, msg_template_map, None)
+
+if __name__ == '__main__':
+    main()