Merge commit '70cda73' into dev

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)

lib/main/CMSIS/CM3/DeviceSupport/ST/STM32F10x/system_stm32f10x.c

@@ -5,7 +5,7 @@
 
 uint32_t SystemCoreClock = SYSCLK_FREQ_72MHz;   /*!< System Clock Frequency (Core Clock) */
 
-__I uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 };
+static const uint8_t AHBPrescTable[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9 };
 
 uint32_t hse_value = 8000000;
 

lib/main/STM32F10x_StdPeriph_Driver/src/stm32f10x_rcc.c

@@ -190,8 +190,8 @@
   * @{
   */ 
 
-static __I uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};
-static __I uint8_t ADCPrescTable[4] = {2, 4, 6, 8};
+static const uint8_t APBAHBPrescTable[16] = {0, 0, 0, 0, 1, 2, 3, 4, 1, 2, 3, 4, 6, 7, 8, 9};
+static const uint8_t ADCPrescTable[4] = {2, 4, 6, 8};
 extern uint32_t hse_value;
 
 /**

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/drivers/gpio.h

@@ -113,6 +113,6 @@ static inline void digitalToggle(GPIO_TypeDef *p, uint16_t i) { p->ODR ^= i; }
 static inline uint16_t digitalIn(GPIO_TypeDef *p, uint16_t i) {return p->IDR & i; }
 #endif
 
-void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config);
+void gpioInit(GPIO_TypeDef *gpio, const gpio_config_t *config);
 void gpioExtiLineConfig(uint8_t portsrc, uint8_t pinsrc);
 void gpioPinRemapConfig(uint32_t remap, bool enable);

src/main/drivers/gpio_stm32f10x.c

@@ -22,7 +22,7 @@
 
 #include "gpio.h"
 
-void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
+void gpioInit(GPIO_TypeDef *gpio, const gpio_config_t *config)
 {
     uint32_t pinpos;
     for (pinpos = 0; pinpos < 16; pinpos++) {

src/main/drivers/gpio_stm32f30x.c

@@ -36,7 +36,7 @@
 //#define GPIO_Speed_2MHz  GPIO_Speed_Level_2   Medium Speed:2MHz
 //#define GPIO_Speed_50MHz GPIO_Speed_Level_3   High Speed:50MHz
 
-void gpioInit(GPIO_TypeDef *gpio, gpio_config_t *config)
+void gpioInit(GPIO_TypeDef *gpio, const gpio_config_t *config)
 {
     GPIO_InitTypeDef GPIO_InitStructure;
 

src/main/drivers/light_led_stm32f10x.c

@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+#include "common/utils.h"
+
 #include "system.h"
 #include "gpio.h"
 
@@ -56,7 +58,7 @@ void ledInit(void)
 #endif
     };
 
-    uint8_t gpio_count = sizeof(gpio_setup) / sizeof(gpio_setup[0]);
+    uint8_t gpio_count = ARRAYLEN(gpio_setup);
 
 #ifdef LED0
     RCC_APB2PeriphClockCmd(LED0_PERIPHERAL, ENABLE);

src/main/drivers/light_led_stm32f30x.c

@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+#include "common/utils.h"
+
 #include "gpio.h"
 
 #include "light_led.h"
@@ -47,7 +49,7 @@ void ledInit(void)
 #endif
     };
 
-    uint8_t gpio_count = sizeof(gpio_setup) / sizeof(gpio_setup[0]);
+    uint8_t gpio_count = ARRAYLEN(gpio_setup);
 
 #ifdef LED0
     RCC_AHBPeriphClockCmd(LED0_PERIPHERAL, ENABLE);

src/main/flight/pid.c

@@ -756,26 +756,26 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     }
 }
 
-void pidSetController(int type)
+void pidSetController(pidControllerType_e type)
 {
     switch (type) {
-        case 0:
+        case PID_CONTROLLER_MULTI_WII:
         default:
             pid_controller = pidMultiWii;
             break;
-        case 1:
+        case PID_CONTROLLER_REWRITE:
             pid_controller = pidRewrite;
             break;
-        case 2:
+        case PID_CONTROLLER_LUX_FLOAT:
             pid_controller = pidLuxFloat;
             break;
-        case 3:
+        case PID_CONTROLLER_MULTI_WII_23:
             pid_controller = pidMultiWii23;
             break;
-        case 4:
+        case PID_CONTROLLER_MULTI_WII_HYBRID:
             pid_controller = pidMultiWiiHybrid;
             break;
-        case 5:
+        case PID_CONTROLLER_HARAKIRI:
             pid_controller = pidHarakiri;
     }
 }

src/main/flight/pid.h

@@ -38,6 +38,15 @@ typedef enum {
     PID_ITEM_COUNT
 } pidIndex_e;
 
+typedef enum {
+    PID_CONTROLLER_MULTI_WII,
+    PID_CONTROLLER_REWRITE,
+    PID_CONTROLLER_LUX_FLOAT,
+    PID_CONTROLLER_MULTI_WII_23,
+    PID_CONTROLLER_MULTI_WII_HYBRID,
+    PID_CONTROLLER_HARAKIRI,
+} pidControllerType_e;
+
 #define IS_PID_CONTROLLER_FP_BASED(pidController) (pidController == 2)
 
 typedef struct pidProfile_s {
@@ -63,7 +72,7 @@ typedef struct pidProfile_s {
 extern int16_t axisPID[XYZ_AXIS_COUNT];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 
-void pidSetController(int type);
+void pidSetController(pidControllerType_e type);
 void pidResetErrorAngle(void);
 void pidResetErrorGyro(void);
 

src/main/io/display.c

@@ -83,7 +83,7 @@ static char lineBuffer[SCREEN_CHARACTER_COLUMN_COUNT + 1];
 #define HALF_SCREEN_CHARACTER_COLUMN_COUNT (SCREEN_CHARACTER_COLUMN_COUNT / 2)
 #define IS_SCREEN_CHARACTER_COLUMN_COUNT_ODD (SCREEN_CHARACTER_COLUMN_COUNT & 1)
 
-const char* pageTitles[] = {
+static const char* const pageTitles[] = {
     "CLEANFLIGHT",
     "ARMED",
     "BATTERY",

src/main/io/gps.c

@@ -28,6 +28,7 @@
 
 #include "common/maths.h"
 #include "common/axis.h"
+#include "common/utils.h"
 
 #include "drivers/system.h"
 #include "drivers/serial.h"
@@ -169,14 +170,14 @@ static const ubloxSbas_t ubloxSbas[] = {
 };
 
 
-enum {
+typedef enum {
     GPS_UNKNOWN,
     GPS_INITIALIZING,
     GPS_CHANGE_BAUD,
     GPS_CONFIGURE,
     GPS_RECEIVING_DATA,
     GPS_LOST_COMMUNICATION,
-};
+} gpsState_e;
 
 gpsData_t gpsData;
 
@@ -185,7 +186,7 @@ static void shiftPacketLog(void)
 {
     uint32_t i;
 
-    for (i = sizeof(gpsPacketLog) - 1; i > 0 ; i--) {
+    for (i = ARRAYLEN(gpsPacketLog) - 1; i > 0 ; i--) {
         gpsPacketLog[i] = gpsPacketLog[i-1];
     }
 }
@@ -194,7 +195,7 @@ static void gpsNewData(uint16_t c);
 static bool gpsNewFrameNMEA(char c);
 static bool gpsNewFrameUBLOX(uint8_t data);
 
-static void gpsSetState(uint8_t state)
+static void gpsSetState(gpsState_e state)
 {
     gpsData.state = state;
     gpsData.state_position = 0;

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

@@ -140,12 +140,12 @@ void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, es
 #define BASEFLIGHT_IDENTIFIER "BAFL";
 
 #define FLIGHT_CONTROLLER_IDENTIFIER_LENGTH 4
-static const char *flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
+static const char * const flightControllerIdentifier = CLEANFLIGHT_IDENTIFIER; // 4 UPPER CASE alpha numeric characters that identify the flight controller.
 
 #define FLIGHT_CONTROLLER_VERSION_LENGTH    3
 #define FLIGHT_CONTROLLER_VERSION_MASK      0xFFF
 
-const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
+static const char * const boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define BOARD_IDENTIFIER_LENGTH             4 // 4 UPPER CASE alpha numeric characters that identify the board being used.
 #define BOARD_HARDWARE_REVISION_LENGTH      2
 
@@ -396,40 +396,31 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 static mspPort_t *currentPort;
 
-static void serialize32(uint32_t a)
-{
-    static uint8_t t;
-    t = a;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-    t = a >> 8;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-    t = a >> 16;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-    t = a >> 24;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-}
-
-static void serialize16(int16_t a)
-{
-    static uint8_t t;
-    t = a;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-    t = a >> 8 & 0xff;
-    serialWrite(mspSerialPort, t);
-    currentPort->checksum ^= t;
-}
-
 static void serialize8(uint8_t a)
 {
     serialWrite(mspSerialPort, 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 void serialize16(uint16_t a)
+{
+    serialize8((uint8_t)(a >> 0));
+    serialize8((uint8_t)(a >> 8));
+}
+
+static void serialize32(uint32_t a)
+{
+    serialize16((uint16_t)(a >> 0));
+    serialize16((uint16_t)(a >> 16));
+}
+
 static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
@@ -699,6 +690,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 +710,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 +811,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);

src/main/version.c

@@ -15,7 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-char *targetName = __TARGET__;
-char *shortGitRevision = __REVISION__;
-char *buildDate = __DATE__;
-char *buildTime = __TIME__;
+const char * const targetName = __TARGET__;
+const char * const shortGitRevision = __REVISION__;
+const char * const buildDate = __DATE__;
+const char * const buildTime = __TIME__;

src/main/version.h

@@ -25,13 +25,13 @@
 
 #define MW_VERSION              231
 
-extern char* targetName;
+extern const char* const targetName;
 
 #define GIT_SHORT_REVISION_LENGTH   7 // lower case hexadecimal digits.
-extern char* shortGitRevision;
+extern const char* const shortGitRevision;
 
 #define BUILD_DATE_LENGTH 11
-extern char* buildDate;  // "MMM DD YYYY" MMM = Jan/Feb/...
+extern const char* const buildDate;  // "MMM DD YYYY" MMM = Jan/Feb/...
 
 #define BUILD_TIME_LENGTH 8
-extern char* buildTime;  // "HH:MM:SS"
+extern const char* const buildTime;  // "HH:MM:SS"

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()