Jacob Hernandez / ros_lib_kinetic

This is a fork from the original, including a small change in the buffer size of the hardware interface (increased to 2048) and decreasing the number of publishers and subscribers to 5. Besides, the library about the message Adc.h was modified so as to increase the number of available Adc channels to be read ( from 6 to 7 ) For this modification, a change in checksum was required

Dependencies:   BufferedSerial

Fork of ros_lib_kinetic by Gary Servin

ros/msg.h@2:9114cc24ddcf, 2017-10-17 (annotated)

Committer:
jacobepfl1692
Date:
Tue Oct 17 18:49:03 2017 +0000
Revision:
2:9114cc24ddcf
Parent:
0:9e9b7db60fd5 
I increased the channels of the ADC to 6 (hence change in checksum) because my application needed it (STM32f407V6)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garyservin 0:9e9b7db60fd5 1 /*
garyservin 0:9e9b7db60fd5 2 * Software License Agreement (BSD License)
garyservin 0:9e9b7db60fd5 3 *
garyservin 0:9e9b7db60fd5 4 * Copyright (c) 2011, Willow Garage, Inc.
garyservin 0:9e9b7db60fd5 5 * All rights reserved.
garyservin 0:9e9b7db60fd5 6 *
garyservin 0:9e9b7db60fd5 7 * Redistribution and use in source and binary forms, with or without
garyservin 0:9e9b7db60fd5 8 * modification, are permitted provided that the following conditions
garyservin 0:9e9b7db60fd5 9 * are met:
garyservin 0:9e9b7db60fd5 10 *
garyservin 0:9e9b7db60fd5 11 * * Redistributions of source code must retain the above copyright
garyservin 0:9e9b7db60fd5 12 * notice, this list of conditions and the following disclaimer.
garyservin 0:9e9b7db60fd5 13 * * Redistributions in binary form must reproduce the above
garyservin 0:9e9b7db60fd5 14 * copyright notice, this list of conditions and the following
garyservin 0:9e9b7db60fd5 15 * disclaimer in the documentation and/or other materials provided
garyservin 0:9e9b7db60fd5 16 * with the distribution.
garyservin 0:9e9b7db60fd5 17 * * Neither the name of Willow Garage, Inc. nor the names of its
garyservin 0:9e9b7db60fd5 18 * contributors may be used to endorse or promote prducts derived
garyservin 0:9e9b7db60fd5 19 * from this software without specific prior written permission.
garyservin 0:9e9b7db60fd5 20 *
garyservin 0:9e9b7db60fd5 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
garyservin 0:9e9b7db60fd5 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
garyservin 0:9e9b7db60fd5 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
garyservin 0:9e9b7db60fd5 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
garyservin 0:9e9b7db60fd5 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
garyservin 0:9e9b7db60fd5 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
garyservin 0:9e9b7db60fd5 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
garyservin 0:9e9b7db60fd5 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
garyservin 0:9e9b7db60fd5 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
garyservin 0:9e9b7db60fd5 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
garyservin 0:9e9b7db60fd5 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
garyservin 0:9e9b7db60fd5 32 * POSSIBILITY OF SUCH DAMAGE.
garyservin 0:9e9b7db60fd5 33 */
garyservin 0:9e9b7db60fd5 34
garyservin 0:9e9b7db60fd5 35 #ifndef _ROS_MSG_H_
garyservin 0:9e9b7db60fd5 36 #define _ROS_MSG_H_
garyservin 0:9e9b7db60fd5 37
garyservin 0:9e9b7db60fd5 38 #include <stdint.h>
garyservin 0:9e9b7db60fd5 39 #include <stddef.h>
garyservin 0:9e9b7db60fd5 40
garyservin 0:9e9b7db60fd5 41 namespace ros {
garyservin 0:9e9b7db60fd5 42
garyservin 0:9e9b7db60fd5 43 /* Base Message Type */
garyservin 0:9e9b7db60fd5 44 class Msg
garyservin 0:9e9b7db60fd5 45 {
garyservin 0:9e9b7db60fd5 46 public:
garyservin 0:9e9b7db60fd5 47 virtual int serialize(unsigned char *outbuffer) const = 0;
garyservin 0:9e9b7db60fd5 48 virtual int deserialize(unsigned char *data) = 0;
garyservin 0:9e9b7db60fd5 49 virtual const char * getType() = 0;
garyservin 0:9e9b7db60fd5 50 virtual const char * getMD5() = 0;
garyservin 0:9e9b7db60fd5 51
garyservin 0:9e9b7db60fd5 52 /**
garyservin 0:9e9b7db60fd5 53 * @brief This tricky function handles promoting a 32bit float to a 64bit
garyservin 0:9e9b7db60fd5 54 * double, so that AVR can publish messages containing float64
garyservin 0:9e9b7db60fd5 55 * fields, despite AVV having no native support for double.
garyservin 0:9e9b7db60fd5 56 *
garyservin 0:9e9b7db60fd5 57 * @param[out] outbuffer pointer for buffer to serialize to.
garyservin 0:9e9b7db60fd5 58 * @param[in] f value to serialize.
garyservin 0:9e9b7db60fd5 59 *
garyservin 0:9e9b7db60fd5 60 * @return number of bytes to advance the buffer pointer.
garyservin 0:9e9b7db60fd5 61 *
garyservin 0:9e9b7db60fd5 62 */
garyservin 0:9e9b7db60fd5 63 static int serializeAvrFloat64(unsigned char* outbuffer, const float f)
garyservin 0:9e9b7db60fd5 64 {
garyservin 0:9e9b7db60fd5 65 const int32_t* val = (int32_t*) &f;
garyservin 0:9e9b7db60fd5 66 int32_t exp = ((*val >> 23) & 255);
garyservin 0:9e9b7db60fd5 67 if (exp != 0)
garyservin 0:9e9b7db60fd5 68 {
garyservin 0:9e9b7db60fd5 69 exp += 1023 - 127;
garyservin 0:9e9b7db60fd5 70 }
garyservin 0:9e9b7db60fd5 71
garyservin 0:9e9b7db60fd5 72 int32_t sig = *val;
garyservin 0:9e9b7db60fd5 73 *(outbuffer++) = 0;
garyservin 0:9e9b7db60fd5 74 *(outbuffer++) = 0;
garyservin 0:9e9b7db60fd5 75 *(outbuffer++) = 0;
garyservin 0:9e9b7db60fd5 76 *(outbuffer++) = (sig << 5) & 0xff;
garyservin 0:9e9b7db60fd5 77 *(outbuffer++) = (sig >> 3) & 0xff;
garyservin 0:9e9b7db60fd5 78 *(outbuffer++) = (sig >> 11) & 0xff;
garyservin 0:9e9b7db60fd5 79 *(outbuffer++) = ((exp << 4) & 0xF0) | ((sig >> 19) & 0x0F);
garyservin 0:9e9b7db60fd5 80 *(outbuffer++) = (exp >> 4) & 0x7F;
garyservin 0:9e9b7db60fd5 81
garyservin 0:9e9b7db60fd5 82 // Mark negative bit as necessary.
garyservin 0:9e9b7db60fd5 83 if (f < 0)
garyservin 0:9e9b7db60fd5 84 {
garyservin 0:9e9b7db60fd5 85 *(outbuffer - 1) |= 0x80;
garyservin 0:9e9b7db60fd5 86 }
garyservin 0:9e9b7db60fd5 87
garyservin 0:9e9b7db60fd5 88 return 8;
garyservin 0:9e9b7db60fd5 89 }
garyservin 0:9e9b7db60fd5 90
garyservin 0:9e9b7db60fd5 91 /**
garyservin 0:9e9b7db60fd5 92 * @brief This tricky function handles demoting a 64bit double to a
garyservin 0:9e9b7db60fd5 93 * 32bit float, so that AVR can understand messages containing
garyservin 0:9e9b7db60fd5 94 * float64 fields, despite AVR having no native support for double.
garyservin 0:9e9b7db60fd5 95 *
garyservin 0:9e9b7db60fd5 96 * @param[in] inbuffer pointer for buffer to deserialize from.
garyservin 0:9e9b7db60fd5 97 * @param[out] f pointer to place the deserialized value in.
garyservin 0:9e9b7db60fd5 98 *
garyservin 0:9e9b7db60fd5 99 * @return number of bytes to advance the buffer pointer.
garyservin 0:9e9b7db60fd5 100 */
garyservin 0:9e9b7db60fd5 101 static int deserializeAvrFloat64(const unsigned char* inbuffer, float* f)
garyservin 0:9e9b7db60fd5 102 {
garyservin 0:9e9b7db60fd5 103 uint32_t* val = (uint32_t*)f;
garyservin 0:9e9b7db60fd5 104 inbuffer += 3;
garyservin 0:9e9b7db60fd5 105
garyservin 0:9e9b7db60fd5 106 // Copy truncated mantissa.
garyservin 0:9e9b7db60fd5 107 *val = ((uint32_t)(*(inbuffer++)) >> 5 & 0x07);
garyservin 0:9e9b7db60fd5 108 *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 3;
garyservin 0:9e9b7db60fd5 109 *val |= ((uint32_t)(*(inbuffer++)) & 0xff) << 11;
garyservin 0:9e9b7db60fd5 110 *val |= ((uint32_t)(*inbuffer) & 0x0f) << 19;
garyservin 0:9e9b7db60fd5 111
garyservin 0:9e9b7db60fd5 112 // Copy truncated exponent.
garyservin 0:9e9b7db60fd5 113 uint32_t exp = ((uint32_t)(*(inbuffer++)) & 0xf0)>>4;
garyservin 0:9e9b7db60fd5 114 exp |= ((uint32_t)(*inbuffer) & 0x7f) << 4;
garyservin 0:9e9b7db60fd5 115 if (exp != 0)
garyservin 0:9e9b7db60fd5 116 {
garyservin 0:9e9b7db60fd5 117 *val |= ((exp) - 1023 + 127) << 23;
garyservin 0:9e9b7db60fd5 118 }
garyservin 0:9e9b7db60fd5 119
garyservin 0:9e9b7db60fd5 120 // Copy negative sign.
garyservin 0:9e9b7db60fd5 121 *val |= ((uint32_t)(*(inbuffer++)) & 0x80) << 24;
garyservin 0:9e9b7db60fd5 122
garyservin 0:9e9b7db60fd5 123 return 8;
garyservin 0:9e9b7db60fd5 124 }
garyservin 0:9e9b7db60fd5 125
garyservin 0:9e9b7db60fd5 126 // Copy data from variable into a byte array
garyservin 0:9e9b7db60fd5 127 template<typename A, typename V>
garyservin 0:9e9b7db60fd5 128 static void varToArr(A arr, const V var)
garyservin 0:9e9b7db60fd5 129 {
garyservin 0:9e9b7db60fd5 130 for(size_t i = 0; i < sizeof(V); i++)
garyservin 0:9e9b7db60fd5 131 arr[i] = (var >> (8 * i));
garyservin 0:9e9b7db60fd5 132 }
garyservin 0:9e9b7db60fd5 133
garyservin 0:9e9b7db60fd5 134 // Copy data from a byte array into variable
garyservin 0:9e9b7db60fd5 135 template<typename V, typename A>
garyservin 0:9e9b7db60fd5 136 static void arrToVar(V& var, const A arr)
garyservin 0:9e9b7db60fd5 137 {
garyservin 0:9e9b7db60fd5 138 var = 0;
garyservin 0:9e9b7db60fd5 139 for(size_t i = 0; i < sizeof(V); i++)
garyservin 0:9e9b7db60fd5 140 var |= (arr[i] << (8 * i));
garyservin 0:9e9b7db60fd5 141 }
garyservin 0:9e9b7db60fd5 142
garyservin 0:9e9b7db60fd5 143 };
garyservin 0:9e9b7db60fd5 144
garyservin 0:9e9b7db60fd5 145 } // namespace ros
garyservin 0:9e9b7db60fd5 146
garyservin 0:9e9b7db60fd5 147 #endif