John Lam / ros_lib_melodic

make changes on buffer serial

Dependencies:   BufferedSerial

Dependents:  

ros/node_handle.h@0:04ac6be8229a, 2019-11-08 (annotated)

Committer:
Gary Servin
Date:
Fri Nov 08 14:38:09 2019 -0300
Revision:
0:04ac6be8229a
Initial commit, generated based on a clean melodic-desktop-full

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Gary Servin 0:04ac6be8229a 1 /*
Gary Servin 0:04ac6be8229a 2 * Software License Agreement (BSD License)
Gary Servin 0:04ac6be8229a 3 *
Gary Servin 0:04ac6be8229a 4 * Copyright (c) 2011, Willow Garage, Inc.
Gary Servin 0:04ac6be8229a 5 * All rights reserved.
Gary Servin 0:04ac6be8229a 6 *
Gary Servin 0:04ac6be8229a 7 * Redistribution and use in source and binary forms, with or without
Gary Servin 0:04ac6be8229a 8 * modification, are permitted provided that the following conditions
Gary Servin 0:04ac6be8229a 9 * are met:
Gary Servin 0:04ac6be8229a 10 *
Gary Servin 0:04ac6be8229a 11 * * Redistributions of source code must retain the above copyright
Gary Servin 0:04ac6be8229a 12 * notice, this list of conditions and the following disclaimer.
Gary Servin 0:04ac6be8229a 13 * * Redistributions in binary form must reproduce the above
Gary Servin 0:04ac6be8229a 14 * copyright notice, this list of conditions and the following
Gary Servin 0:04ac6be8229a 15 * disclaimer in the documentation and/or other materials provided
Gary Servin 0:04ac6be8229a 16 * with the distribution.
Gary Servin 0:04ac6be8229a 17 * * Neither the name of Willow Garage, Inc. nor the names of its
Gary Servin 0:04ac6be8229a 18 * contributors may be used to endorse or promote prducts derived
Gary Servin 0:04ac6be8229a 19 * from this software without specific prior written permission.
Gary Servin 0:04ac6be8229a 20 *
Gary Servin 0:04ac6be8229a 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
Gary Servin 0:04ac6be8229a 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
Gary Servin 0:04ac6be8229a 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
Gary Servin 0:04ac6be8229a 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
Gary Servin 0:04ac6be8229a 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
Gary Servin 0:04ac6be8229a 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
Gary Servin 0:04ac6be8229a 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
Gary Servin 0:04ac6be8229a 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Gary Servin 0:04ac6be8229a 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
Gary Servin 0:04ac6be8229a 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
Gary Servin 0:04ac6be8229a 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
Gary Servin 0:04ac6be8229a 32 * POSSIBILITY OF SUCH DAMAGE.
Gary Servin 0:04ac6be8229a 33 */
Gary Servin 0:04ac6be8229a 34
Gary Servin 0:04ac6be8229a 35 #ifndef ROS_NODE_HANDLE_H_
Gary Servin 0:04ac6be8229a 36 #define ROS_NODE_HANDLE_H_
Gary Servin 0:04ac6be8229a 37
Gary Servin 0:04ac6be8229a 38 #include <stdint.h>
Gary Servin 0:04ac6be8229a 39
Gary Servin 0:04ac6be8229a 40 #include "std_msgs/Time.h"
Gary Servin 0:04ac6be8229a 41 #include "rosserial_msgs/TopicInfo.h"
Gary Servin 0:04ac6be8229a 42 #include "rosserial_msgs/Log.h"
Gary Servin 0:04ac6be8229a 43 #include "rosserial_msgs/RequestParam.h"
Gary Servin 0:04ac6be8229a 44
Gary Servin 0:04ac6be8229a 45 #include "ros/msg.h"
Gary Servin 0:04ac6be8229a 46
Gary Servin 0:04ac6be8229a 47 namespace ros
Gary Servin 0:04ac6be8229a 48 {
Gary Servin 0:04ac6be8229a 49
Gary Servin 0:04ac6be8229a 50 class NodeHandleBase_
Gary Servin 0:04ac6be8229a 51 {
Gary Servin 0:04ac6be8229a 52 public:
Gary Servin 0:04ac6be8229a 53 virtual int publish(int id, const Msg* msg) = 0;
Gary Servin 0:04ac6be8229a 54 virtual int spinOnce() = 0;
Gary Servin 0:04ac6be8229a 55 virtual bool connected() = 0;
Gary Servin 0:04ac6be8229a 56 };
Gary Servin 0:04ac6be8229a 57 }
Gary Servin 0:04ac6be8229a 58
Gary Servin 0:04ac6be8229a 59 #include "ros/publisher.h"
Gary Servin 0:04ac6be8229a 60 #include "ros/subscriber.h"
Gary Servin 0:04ac6be8229a 61 #include "ros/service_server.h"
Gary Servin 0:04ac6be8229a 62 #include "ros/service_client.h"
Gary Servin 0:04ac6be8229a 63
Gary Servin 0:04ac6be8229a 64 namespace ros
Gary Servin 0:04ac6be8229a 65 {
Gary Servin 0:04ac6be8229a 66
Gary Servin 0:04ac6be8229a 67 const int SPIN_OK = 0;
Gary Servin 0:04ac6be8229a 68 const int SPIN_ERR = -1;
Gary Servin 0:04ac6be8229a 69 const int SPIN_TIMEOUT = -2;
Gary Servin 0:04ac6be8229a 70
Gary Servin 0:04ac6be8229a 71 const uint8_t SYNC_SECONDS = 5;
Gary Servin 0:04ac6be8229a 72 const uint8_t MODE_FIRST_FF = 0;
Gary Servin 0:04ac6be8229a 73 /*
Gary Servin 0:04ac6be8229a 74 * The second sync byte is a protocol version. It's value is 0xff for the first
Gary Servin 0:04ac6be8229a 75 * version of the rosserial protocol (used up to hydro), 0xfe for the second version
Gary Servin 0:04ac6be8229a 76 * (introduced in hydro), 0xfd for the next, and so on. Its purpose is to enable
Gary Servin 0:04ac6be8229a 77 * detection of mismatched protocol versions (e.g. hydro rosserial_python with groovy
Gary Servin 0:04ac6be8229a 78 * rosserial_arduino. It must be changed in both this file and in
Gary Servin 0:04ac6be8229a 79 * rosserial_python/src/rosserial_python/SerialClient.py
Gary Servin 0:04ac6be8229a 80 */
Gary Servin 0:04ac6be8229a 81 const uint8_t MODE_PROTOCOL_VER = 1;
Gary Servin 0:04ac6be8229a 82 const uint8_t PROTOCOL_VER1 = 0xff; // through groovy
Gary Servin 0:04ac6be8229a 83 const uint8_t PROTOCOL_VER2 = 0xfe; // in hydro
Gary Servin 0:04ac6be8229a 84 const uint8_t PROTOCOL_VER = PROTOCOL_VER2;
Gary Servin 0:04ac6be8229a 85 const uint8_t MODE_SIZE_L = 2;
Gary Servin 0:04ac6be8229a 86 const uint8_t MODE_SIZE_H = 3;
Gary Servin 0:04ac6be8229a 87 const uint8_t MODE_SIZE_CHECKSUM = 4; // checksum for msg size received from size L and H
Gary Servin 0:04ac6be8229a 88 const uint8_t MODE_TOPIC_L = 5; // waiting for topic id
Gary Servin 0:04ac6be8229a 89 const uint8_t MODE_TOPIC_H = 6;
Gary Servin 0:04ac6be8229a 90 const uint8_t MODE_MESSAGE = 7;
Gary Servin 0:04ac6be8229a 91 const uint8_t MODE_MSG_CHECKSUM = 8; // checksum for msg and topic id
Gary Servin 0:04ac6be8229a 92
Gary Servin 0:04ac6be8229a 93
Gary Servin 0:04ac6be8229a 94 const uint8_t SERIAL_MSG_TIMEOUT = 20; // 20 milliseconds to recieve all of message data
Gary Servin 0:04ac6be8229a 95
Gary Servin 0:04ac6be8229a 96 using rosserial_msgs::TopicInfo;
Gary Servin 0:04ac6be8229a 97
Gary Servin 0:04ac6be8229a 98 /* Node Handle */
Gary Servin 0:04ac6be8229a 99 template<class Hardware,
Gary Servin 0:04ac6be8229a 100 int MAX_SUBSCRIBERS = 25,
Gary Servin 0:04ac6be8229a 101 int MAX_PUBLISHERS = 25,
Gary Servin 0:04ac6be8229a 102 int INPUT_SIZE = 512,
Gary Servin 0:04ac6be8229a 103 int OUTPUT_SIZE = 512>
Gary Servin 0:04ac6be8229a 104 class NodeHandle_ : public NodeHandleBase_
Gary Servin 0:04ac6be8229a 105 {
Gary Servin 0:04ac6be8229a 106 protected:
Gary Servin 0:04ac6be8229a 107 Hardware hardware_;
Gary Servin 0:04ac6be8229a 108
Gary Servin 0:04ac6be8229a 109 /* time used for syncing */
Gary Servin 0:04ac6be8229a 110 uint32_t rt_time;
Gary Servin 0:04ac6be8229a 111
Gary Servin 0:04ac6be8229a 112 /* used for computing current time */
Gary Servin 0:04ac6be8229a 113 uint32_t sec_offset, nsec_offset;
Gary Servin 0:04ac6be8229a 114
Gary Servin 0:04ac6be8229a 115 /* Spinonce maximum work timeout */
Gary Servin 0:04ac6be8229a 116 uint32_t spin_timeout_;
Gary Servin 0:04ac6be8229a 117
Gary Servin 0:04ac6be8229a 118 uint8_t message_in[INPUT_SIZE];
Gary Servin 0:04ac6be8229a 119 uint8_t message_out[OUTPUT_SIZE];
Gary Servin 0:04ac6be8229a 120
Gary Servin 0:04ac6be8229a 121 Publisher * publishers[MAX_PUBLISHERS];
Gary Servin 0:04ac6be8229a 122 Subscriber_ * subscribers[MAX_SUBSCRIBERS];
Gary Servin 0:04ac6be8229a 123
Gary Servin 0:04ac6be8229a 124 /*
Gary Servin 0:04ac6be8229a 125 * Setup Functions
Gary Servin 0:04ac6be8229a 126 */
Gary Servin 0:04ac6be8229a 127 public:
Gary Servin 0:04ac6be8229a 128 NodeHandle_() : configured_(false)
Gary Servin 0:04ac6be8229a 129 {
Gary Servin 0:04ac6be8229a 130
Gary Servin 0:04ac6be8229a 131 for (unsigned int i = 0; i < MAX_PUBLISHERS; i++)
Gary Servin 0:04ac6be8229a 132 publishers[i] = 0;
Gary Servin 0:04ac6be8229a 133
Gary Servin 0:04ac6be8229a 134 for (unsigned int i = 0; i < MAX_SUBSCRIBERS; i++)
Gary Servin 0:04ac6be8229a 135 subscribers[i] = 0;
Gary Servin 0:04ac6be8229a 136
Gary Servin 0:04ac6be8229a 137 for (unsigned int i = 0; i < INPUT_SIZE; i++)
Gary Servin 0:04ac6be8229a 138 message_in[i] = 0;
Gary Servin 0:04ac6be8229a 139
Gary Servin 0:04ac6be8229a 140 for (unsigned int i = 0; i < OUTPUT_SIZE; i++)
Gary Servin 0:04ac6be8229a 141 message_out[i] = 0;
Gary Servin 0:04ac6be8229a 142
Gary Servin 0:04ac6be8229a 143 req_param_resp.ints_length = 0;
Gary Servin 0:04ac6be8229a 144 req_param_resp.ints = NULL;
Gary Servin 0:04ac6be8229a 145 req_param_resp.floats_length = 0;
Gary Servin 0:04ac6be8229a 146 req_param_resp.floats = NULL;
Gary Servin 0:04ac6be8229a 147 req_param_resp.ints_length = 0;
Gary Servin 0:04ac6be8229a 148 req_param_resp.ints = NULL;
Gary Servin 0:04ac6be8229a 149
Gary Servin 0:04ac6be8229a 150 spin_timeout_ = 0;
Gary Servin 0:04ac6be8229a 151 }
Gary Servin 0:04ac6be8229a 152
Gary Servin 0:04ac6be8229a 153 Hardware* getHardware()
Gary Servin 0:04ac6be8229a 154 {
Gary Servin 0:04ac6be8229a 155 return &hardware_;
Gary Servin 0:04ac6be8229a 156 }
Gary Servin 0:04ac6be8229a 157
Gary Servin 0:04ac6be8229a 158 /* Start serial, initialize buffers */
Gary Servin 0:04ac6be8229a 159 void initNode()
Gary Servin 0:04ac6be8229a 160 {
Gary Servin 0:04ac6be8229a 161 hardware_.init();
Gary Servin 0:04ac6be8229a 162 mode_ = 0;
Gary Servin 0:04ac6be8229a 163 bytes_ = 0;
Gary Servin 0:04ac6be8229a 164 index_ = 0;
Gary Servin 0:04ac6be8229a 165 topic_ = 0;
Gary Servin 0:04ac6be8229a 166 };
Gary Servin 0:04ac6be8229a 167
Gary Servin 0:04ac6be8229a 168 /* Start a named port, which may be network server IP, initialize buffers */
Gary Servin 0:04ac6be8229a 169 void initNode(char *portName)
Gary Servin 0:04ac6be8229a 170 {
Gary Servin 0:04ac6be8229a 171 hardware_.init(portName);
Gary Servin 0:04ac6be8229a 172 mode_ = 0;
Gary Servin 0:04ac6be8229a 173 bytes_ = 0;
Gary Servin 0:04ac6be8229a 174 index_ = 0;
Gary Servin 0:04ac6be8229a 175 topic_ = 0;
Gary Servin 0:04ac6be8229a 176 };
Gary Servin 0:04ac6be8229a 177
Gary Servin 0:04ac6be8229a 178 /**
Gary Servin 0:04ac6be8229a 179 * @brief Sets the maximum time in millisconds that spinOnce() can work.
Gary Servin 0:04ac6be8229a 180 * This will not effect the processing of the buffer, as spinOnce processes
Gary Servin 0:04ac6be8229a 181 * one byte at a time. It simply sets the maximum time that one call can
Gary Servin 0:04ac6be8229a 182 * process for. You can choose to clear the buffer if that is beneficial if
Gary Servin 0:04ac6be8229a 183 * SPIN_TIMEOUT is returned from spinOnce().
Gary Servin 0:04ac6be8229a 184 * @param timeout The timeout in milliseconds that spinOnce will function.
Gary Servin 0:04ac6be8229a 185 */
Gary Servin 0:04ac6be8229a 186 void setSpinTimeout(const uint32_t& timeout)
Gary Servin 0:04ac6be8229a 187 {
Gary Servin 0:04ac6be8229a 188 spin_timeout_ = timeout;
Gary Servin 0:04ac6be8229a 189 }
Gary Servin 0:04ac6be8229a 190
Gary Servin 0:04ac6be8229a 191 protected:
Gary Servin 0:04ac6be8229a 192 //State machine variables for spinOnce
Gary Servin 0:04ac6be8229a 193 int mode_;
Gary Servin 0:04ac6be8229a 194 int bytes_;
Gary Servin 0:04ac6be8229a 195 int topic_;
Gary Servin 0:04ac6be8229a 196 int index_;
Gary Servin 0:04ac6be8229a 197 int checksum_;
Gary Servin 0:04ac6be8229a 198
Gary Servin 0:04ac6be8229a 199 bool configured_;
Gary Servin 0:04ac6be8229a 200
Gary Servin 0:04ac6be8229a 201 /* used for syncing the time */
Gary Servin 0:04ac6be8229a 202 uint32_t last_sync_time;
Gary Servin 0:04ac6be8229a 203 uint32_t last_sync_receive_time;
Gary Servin 0:04ac6be8229a 204 uint32_t last_msg_timeout_time;
Gary Servin 0:04ac6be8229a 205
Gary Servin 0:04ac6be8229a 206 public:
Gary Servin 0:04ac6be8229a 207 /* This function goes in your loop() function, it handles
Gary Servin 0:04ac6be8229a 208 * serial input and callbacks for subscribers.
Gary Servin 0:04ac6be8229a 209 */
Gary Servin 0:04ac6be8229a 210
Gary Servin 0:04ac6be8229a 211
Gary Servin 0:04ac6be8229a 212 virtual int spinOnce()
Gary Servin 0:04ac6be8229a 213 {
Gary Servin 0:04ac6be8229a 214 /* restart if timed out */
Gary Servin 0:04ac6be8229a 215 uint32_t c_time = hardware_.time();
Gary Servin 0:04ac6be8229a 216 if ((c_time - last_sync_receive_time) > (SYNC_SECONDS * 2200))
Gary Servin 0:04ac6be8229a 217 {
Gary Servin 0:04ac6be8229a 218 configured_ = false;
Gary Servin 0:04ac6be8229a 219 }
Gary Servin 0:04ac6be8229a 220
Gary Servin 0:04ac6be8229a 221 /* reset if message has timed out */
Gary Servin 0:04ac6be8229a 222 if (mode_ != MODE_FIRST_FF)
Gary Servin 0:04ac6be8229a 223 {
Gary Servin 0:04ac6be8229a 224 if (c_time > last_msg_timeout_time)
Gary Servin 0:04ac6be8229a 225 {
Gary Servin 0:04ac6be8229a 226 mode_ = MODE_FIRST_FF;
Gary Servin 0:04ac6be8229a 227 }
Gary Servin 0:04ac6be8229a 228 }
Gary Servin 0:04ac6be8229a 229
Gary Servin 0:04ac6be8229a 230 /* while available buffer, read data */
Gary Servin 0:04ac6be8229a 231 while (true)
Gary Servin 0:04ac6be8229a 232 {
Gary Servin 0:04ac6be8229a 233 // If a timeout has been specified, check how long spinOnce has been running.
Gary Servin 0:04ac6be8229a 234 if (spin_timeout_ > 0)
Gary Servin 0:04ac6be8229a 235 {
Gary Servin 0:04ac6be8229a 236 // If the maximum processing timeout has been exceeded, exit with error.
Gary Servin 0:04ac6be8229a 237 // The next spinOnce can continue where it left off, or optionally
Gary Servin 0:04ac6be8229a 238 // based on the application in use, the hardware buffer could be flushed
Gary Servin 0:04ac6be8229a 239 // and start fresh.
Gary Servin 0:04ac6be8229a 240 if ((hardware_.time() - c_time) > spin_timeout_)
Gary Servin 0:04ac6be8229a 241 {
Gary Servin 0:04ac6be8229a 242 // Exit the spin, processing timeout exceeded.
Gary Servin 0:04ac6be8229a 243 return SPIN_TIMEOUT;
Gary Servin 0:04ac6be8229a 244 }
Gary Servin 0:04ac6be8229a 245 }
Gary Servin 0:04ac6be8229a 246 int data = hardware_.read();
Gary Servin 0:04ac6be8229a 247 if (data < 0)
Gary Servin 0:04ac6be8229a 248 break;
Gary Servin 0:04ac6be8229a 249 checksum_ += data;
Gary Servin 0:04ac6be8229a 250 if (mode_ == MODE_MESSAGE) /* message data being recieved */
Gary Servin 0:04ac6be8229a 251 {
Gary Servin 0:04ac6be8229a 252 message_in[index_++] = data;
Gary Servin 0:04ac6be8229a 253 bytes_--;
Gary Servin 0:04ac6be8229a 254 if (bytes_ == 0) /* is message complete? if so, checksum */
Gary Servin 0:04ac6be8229a 255 mode_ = MODE_MSG_CHECKSUM;
Gary Servin 0:04ac6be8229a 256 }
Gary Servin 0:04ac6be8229a 257 else if (mode_ == MODE_FIRST_FF)
Gary Servin 0:04ac6be8229a 258 {
Gary Servin 0:04ac6be8229a 259 if (data == 0xff)
Gary Servin 0:04ac6be8229a 260 {
Gary Servin 0:04ac6be8229a 261 mode_++;
Gary Servin 0:04ac6be8229a 262 last_msg_timeout_time = c_time + SERIAL_MSG_TIMEOUT;
Gary Servin 0:04ac6be8229a 263 }
Gary Servin 0:04ac6be8229a 264 else if (hardware_.time() - c_time > (SYNC_SECONDS * 1000))
Gary Servin 0:04ac6be8229a 265 {
Gary Servin 0:04ac6be8229a 266 /* We have been stuck in spinOnce too long, return error */
Gary Servin 0:04ac6be8229a 267 configured_ = false;
Gary Servin 0:04ac6be8229a 268 return SPIN_TIMEOUT;
Gary Servin 0:04ac6be8229a 269 }
Gary Servin 0:04ac6be8229a 270 }
Gary Servin 0:04ac6be8229a 271 else if (mode_ == MODE_PROTOCOL_VER)
Gary Servin 0:04ac6be8229a 272 {
Gary Servin 0:04ac6be8229a 273 if (data == PROTOCOL_VER)
Gary Servin 0:04ac6be8229a 274 {
Gary Servin 0:04ac6be8229a 275 mode_++;
Gary Servin 0:04ac6be8229a 276 }
Gary Servin 0:04ac6be8229a 277 else
Gary Servin 0:04ac6be8229a 278 {
Gary Servin 0:04ac6be8229a 279 mode_ = MODE_FIRST_FF;
Gary Servin 0:04ac6be8229a 280 if (configured_ == false)
Gary Servin 0:04ac6be8229a 281 requestSyncTime(); /* send a msg back showing our protocol version */
Gary Servin 0:04ac6be8229a 282 }
Gary Servin 0:04ac6be8229a 283 }
Gary Servin 0:04ac6be8229a 284 else if (mode_ == MODE_SIZE_L) /* bottom half of message size */
Gary Servin 0:04ac6be8229a 285 {
Gary Servin 0:04ac6be8229a 286 bytes_ = data;
Gary Servin 0:04ac6be8229a 287 index_ = 0;
Gary Servin 0:04ac6be8229a 288 mode_++;
Gary Servin 0:04ac6be8229a 289 checksum_ = data; /* first byte for calculating size checksum */
Gary Servin 0:04ac6be8229a 290 }
Gary Servin 0:04ac6be8229a 291 else if (mode_ == MODE_SIZE_H) /* top half of message size */
Gary Servin 0:04ac6be8229a 292 {
Gary Servin 0:04ac6be8229a 293 bytes_ += data << 8;
Gary Servin 0:04ac6be8229a 294 mode_++;
Gary Servin 0:04ac6be8229a 295 }
Gary Servin 0:04ac6be8229a 296 else if (mode_ == MODE_SIZE_CHECKSUM)
Gary Servin 0:04ac6be8229a 297 {
Gary Servin 0:04ac6be8229a 298 if ((checksum_ % 256) == 255)
Gary Servin 0:04ac6be8229a 299 mode_++;
Gary Servin 0:04ac6be8229a 300 else
Gary Servin 0:04ac6be8229a 301 mode_ = MODE_FIRST_FF; /* Abandon the frame if the msg len is wrong */
Gary Servin 0:04ac6be8229a 302 }
Gary Servin 0:04ac6be8229a 303 else if (mode_ == MODE_TOPIC_L) /* bottom half of topic id */
Gary Servin 0:04ac6be8229a 304 {
Gary Servin 0:04ac6be8229a 305 topic_ = data;
Gary Servin 0:04ac6be8229a 306 mode_++;
Gary Servin 0:04ac6be8229a 307 checksum_ = data; /* first byte included in checksum */
Gary Servin 0:04ac6be8229a 308 }
Gary Servin 0:04ac6be8229a 309 else if (mode_ == MODE_TOPIC_H) /* top half of topic id */
Gary Servin 0:04ac6be8229a 310 {
Gary Servin 0:04ac6be8229a 311 topic_ += data << 8;
Gary Servin 0:04ac6be8229a 312 mode_ = MODE_MESSAGE;
Gary Servin 0:04ac6be8229a 313 if (bytes_ == 0)
Gary Servin 0:04ac6be8229a 314 mode_ = MODE_MSG_CHECKSUM;
Gary Servin 0:04ac6be8229a 315 }
Gary Servin 0:04ac6be8229a 316 else if (mode_ == MODE_MSG_CHECKSUM) /* do checksum */
Gary Servin 0:04ac6be8229a 317 {
Gary Servin 0:04ac6be8229a 318 mode_ = MODE_FIRST_FF;
Gary Servin 0:04ac6be8229a 319 if ((checksum_ % 256) == 255)
Gary Servin 0:04ac6be8229a 320 {
Gary Servin 0:04ac6be8229a 321 if (topic_ == TopicInfo::ID_PUBLISHER)
Gary Servin 0:04ac6be8229a 322 {
Gary Servin 0:04ac6be8229a 323 requestSyncTime();
Gary Servin 0:04ac6be8229a 324 negotiateTopics();
Gary Servin 0:04ac6be8229a 325 last_sync_time = c_time;
Gary Servin 0:04ac6be8229a 326 last_sync_receive_time = c_time;
Gary Servin 0:04ac6be8229a 327 return SPIN_ERR;
Gary Servin 0:04ac6be8229a 328 }
Gary Servin 0:04ac6be8229a 329 else if (topic_ == TopicInfo::ID_TIME)
Gary Servin 0:04ac6be8229a 330 {
Gary Servin 0:04ac6be8229a 331 syncTime(message_in);
Gary Servin 0:04ac6be8229a 332 }
Gary Servin 0:04ac6be8229a 333 else if (topic_ == TopicInfo::ID_PARAMETER_REQUEST)
Gary Servin 0:04ac6be8229a 334 {
Gary Servin 0:04ac6be8229a 335 req_param_resp.deserialize(message_in);
Gary Servin 0:04ac6be8229a 336 param_recieved = true;
Gary Servin 0:04ac6be8229a 337 }
Gary Servin 0:04ac6be8229a 338 else if (topic_ == TopicInfo::ID_TX_STOP)
Gary Servin 0:04ac6be8229a 339 {
Gary Servin 0:04ac6be8229a 340 configured_ = false;
Gary Servin 0:04ac6be8229a 341 }
Gary Servin 0:04ac6be8229a 342 else
Gary Servin 0:04ac6be8229a 343 {
Gary Servin 0:04ac6be8229a 344 if (subscribers[topic_ - 100])
Gary Servin 0:04ac6be8229a 345 subscribers[topic_ - 100]->callback(message_in);
Gary Servin 0:04ac6be8229a 346 }
Gary Servin 0:04ac6be8229a 347 }
Gary Servin 0:04ac6be8229a 348 }
Gary Servin 0:04ac6be8229a 349 }
Gary Servin 0:04ac6be8229a 350
Gary Servin 0:04ac6be8229a 351 /* occasionally sync time */
Gary Servin 0:04ac6be8229a 352 if (configured_ && ((c_time - last_sync_time) > (SYNC_SECONDS * 500)))
Gary Servin 0:04ac6be8229a 353 {
Gary Servin 0:04ac6be8229a 354 requestSyncTime();
Gary Servin 0:04ac6be8229a 355 last_sync_time = c_time;
Gary Servin 0:04ac6be8229a 356 }
Gary Servin 0:04ac6be8229a 357
Gary Servin 0:04ac6be8229a 358 return SPIN_OK;
Gary Servin 0:04ac6be8229a 359 }
Gary Servin 0:04ac6be8229a 360
Gary Servin 0:04ac6be8229a 361
Gary Servin 0:04ac6be8229a 362 /* Are we connected to the PC? */
Gary Servin 0:04ac6be8229a 363 virtual bool connected()
Gary Servin 0:04ac6be8229a 364 {
Gary Servin 0:04ac6be8229a 365 return configured_;
Gary Servin 0:04ac6be8229a 366 };
Gary Servin 0:04ac6be8229a 367
Gary Servin 0:04ac6be8229a 368 /********************************************************************
Gary Servin 0:04ac6be8229a 369 * Time functions
Gary Servin 0:04ac6be8229a 370 */
Gary Servin 0:04ac6be8229a 371
Gary Servin 0:04ac6be8229a 372 void requestSyncTime()
Gary Servin 0:04ac6be8229a 373 {
Gary Servin 0:04ac6be8229a 374 std_msgs::Time t;
Gary Servin 0:04ac6be8229a 375 publish(TopicInfo::ID_TIME, &t);
Gary Servin 0:04ac6be8229a 376 rt_time = hardware_.time();
Gary Servin 0:04ac6be8229a 377 }
Gary Servin 0:04ac6be8229a 378
Gary Servin 0:04ac6be8229a 379 void syncTime(uint8_t * data)
Gary Servin 0:04ac6be8229a 380 {
Gary Servin 0:04ac6be8229a 381 std_msgs::Time t;
Gary Servin 0:04ac6be8229a 382 uint32_t offset = hardware_.time() - rt_time;
Gary Servin 0:04ac6be8229a 383
Gary Servin 0:04ac6be8229a 384 t.deserialize(data);
Gary Servin 0:04ac6be8229a 385 t.data.sec += offset / 1000;
Gary Servin 0:04ac6be8229a 386 t.data.nsec += (offset % 1000) * 1000000UL;
Gary Servin 0:04ac6be8229a 387
Gary Servin 0:04ac6be8229a 388 this->setNow(t.data);
Gary Servin 0:04ac6be8229a 389 last_sync_receive_time = hardware_.time();
Gary Servin 0:04ac6be8229a 390 }
Gary Servin 0:04ac6be8229a 391
Gary Servin 0:04ac6be8229a 392 Time now()
Gary Servin 0:04ac6be8229a 393 {
Gary Servin 0:04ac6be8229a 394 uint32_t ms = hardware_.time();
Gary Servin 0:04ac6be8229a 395 Time current_time;
Gary Servin 0:04ac6be8229a 396 current_time.sec = ms / 1000 + sec_offset;
Gary Servin 0:04ac6be8229a 397 current_time.nsec = (ms % 1000) * 1000000UL + nsec_offset;
Gary Servin 0:04ac6be8229a 398 normalizeSecNSec(current_time.sec, current_time.nsec);
Gary Servin 0:04ac6be8229a 399 return current_time;
Gary Servin 0:04ac6be8229a 400 }
Gary Servin 0:04ac6be8229a 401
Gary Servin 0:04ac6be8229a 402 void setNow(Time & new_now)
Gary Servin 0:04ac6be8229a 403 {
Gary Servin 0:04ac6be8229a 404 uint32_t ms = hardware_.time();
Gary Servin 0:04ac6be8229a 405 sec_offset = new_now.sec - ms / 1000 - 1;
Gary Servin 0:04ac6be8229a 406 nsec_offset = new_now.nsec - (ms % 1000) * 1000000UL + 1000000000UL;
Gary Servin 0:04ac6be8229a 407 normalizeSecNSec(sec_offset, nsec_offset);
Gary Servin 0:04ac6be8229a 408 }
Gary Servin 0:04ac6be8229a 409
Gary Servin 0:04ac6be8229a 410 /********************************************************************
Gary Servin 0:04ac6be8229a 411 * Topic Management
Gary Servin 0:04ac6be8229a 412 */
Gary Servin 0:04ac6be8229a 413
Gary Servin 0:04ac6be8229a 414 /* Register a new publisher */
Gary Servin 0:04ac6be8229a 415 bool advertise(Publisher & p)
Gary Servin 0:04ac6be8229a 416 {
Gary Servin 0:04ac6be8229a 417 for (int i = 0; i < MAX_PUBLISHERS; i++)
Gary Servin 0:04ac6be8229a 418 {
Gary Servin 0:04ac6be8229a 419 if (publishers[i] == 0) // empty slot
Gary Servin 0:04ac6be8229a 420 {
Gary Servin 0:04ac6be8229a 421 publishers[i] = &p;
Gary Servin 0:04ac6be8229a 422 p.id_ = i + 100 + MAX_SUBSCRIBERS;
Gary Servin 0:04ac6be8229a 423 p.nh_ = this;
Gary Servin 0:04ac6be8229a 424 return true;
Gary Servin 0:04ac6be8229a 425 }
Gary Servin 0:04ac6be8229a 426 }
Gary Servin 0:04ac6be8229a 427 return false;
Gary Servin 0:04ac6be8229a 428 }
Gary Servin 0:04ac6be8229a 429
Gary Servin 0:04ac6be8229a 430 /* Register a new subscriber */
Gary Servin 0:04ac6be8229a 431 template<typename SubscriberT>
Gary Servin 0:04ac6be8229a 432 bool subscribe(SubscriberT& s)
Gary Servin 0:04ac6be8229a 433 {
Gary Servin 0:04ac6be8229a 434 for (int i = 0; i < MAX_SUBSCRIBERS; i++)
Gary Servin 0:04ac6be8229a 435 {
Gary Servin 0:04ac6be8229a 436 if (subscribers[i] == 0) // empty slot
Gary Servin 0:04ac6be8229a 437 {
Gary Servin 0:04ac6be8229a 438 subscribers[i] = static_cast<Subscriber_*>(&s);
Gary Servin 0:04ac6be8229a 439 s.id_ = i + 100;
Gary Servin 0:04ac6be8229a 440 return true;
Gary Servin 0:04ac6be8229a 441 }
Gary Servin 0:04ac6be8229a 442 }
Gary Servin 0:04ac6be8229a 443 return false;
Gary Servin 0:04ac6be8229a 444 }
Gary Servin 0:04ac6be8229a 445
Gary Servin 0:04ac6be8229a 446 /* Register a new Service Server */
Gary Servin 0:04ac6be8229a 447 template<typename MReq, typename MRes, typename ObjT>
Gary Servin 0:04ac6be8229a 448 bool advertiseService(ServiceServer<MReq, MRes, ObjT>& srv)
Gary Servin 0:04ac6be8229a 449 {
Gary Servin 0:04ac6be8229a 450 bool v = advertise(srv.pub);
Gary Servin 0:04ac6be8229a 451 for (int i = 0; i < MAX_SUBSCRIBERS; i++)
Gary Servin 0:04ac6be8229a 452 {
Gary Servin 0:04ac6be8229a 453 if (subscribers[i] == 0) // empty slot
Gary Servin 0:04ac6be8229a 454 {
Gary Servin 0:04ac6be8229a 455 subscribers[i] = static_cast<Subscriber_*>(&srv);
Gary Servin 0:04ac6be8229a 456 srv.id_ = i + 100;
Gary Servin 0:04ac6be8229a 457 return v;
Gary Servin 0:04ac6be8229a 458 }
Gary Servin 0:04ac6be8229a 459 }
Gary Servin 0:04ac6be8229a 460 return false;
Gary Servin 0:04ac6be8229a 461 }
Gary Servin 0:04ac6be8229a 462
Gary Servin 0:04ac6be8229a 463 /* Register a new Service Client */
Gary Servin 0:04ac6be8229a 464 template<typename MReq, typename MRes>
Gary Servin 0:04ac6be8229a 465 bool serviceClient(ServiceClient<MReq, MRes>& srv)
Gary Servin 0:04ac6be8229a 466 {
Gary Servin 0:04ac6be8229a 467 bool v = advertise(srv.pub);
Gary Servin 0:04ac6be8229a 468 for (int i = 0; i < MAX_SUBSCRIBERS; i++)
Gary Servin 0:04ac6be8229a 469 {
Gary Servin 0:04ac6be8229a 470 if (subscribers[i] == 0) // empty slot
Gary Servin 0:04ac6be8229a 471 {
Gary Servin 0:04ac6be8229a 472 subscribers[i] = static_cast<Subscriber_*>(&srv);
Gary Servin 0:04ac6be8229a 473 srv.id_ = i + 100;
Gary Servin 0:04ac6be8229a 474 return v;
Gary Servin 0:04ac6be8229a 475 }
Gary Servin 0:04ac6be8229a 476 }
Gary Servin 0:04ac6be8229a 477 return false;
Gary Servin 0:04ac6be8229a 478 }
Gary Servin 0:04ac6be8229a 479
Gary Servin 0:04ac6be8229a 480 void negotiateTopics()
Gary Servin 0:04ac6be8229a 481 {
Gary Servin 0:04ac6be8229a 482 rosserial_msgs::TopicInfo ti;
Gary Servin 0:04ac6be8229a 483 int i;
Gary Servin 0:04ac6be8229a 484 for (i = 0; i < MAX_PUBLISHERS; i++)
Gary Servin 0:04ac6be8229a 485 {
Gary Servin 0:04ac6be8229a 486 if (publishers[i] != 0) // non-empty slot
Gary Servin 0:04ac6be8229a 487 {
Gary Servin 0:04ac6be8229a 488 ti.topic_id = publishers[i]->id_;
Gary Servin 0:04ac6be8229a 489 ti.topic_name = (char *) publishers[i]->topic_;
Gary Servin 0:04ac6be8229a 490 ti.message_type = (char *) publishers[i]->msg_->getType();
Gary Servin 0:04ac6be8229a 491 ti.md5sum = (char *) publishers[i]->msg_->getMD5();
Gary Servin 0:04ac6be8229a 492 ti.buffer_size = OUTPUT_SIZE;
Gary Servin 0:04ac6be8229a 493 publish(publishers[i]->getEndpointType(), &ti);
Gary Servin 0:04ac6be8229a 494 }
Gary Servin 0:04ac6be8229a 495 }
Gary Servin 0:04ac6be8229a 496 for (i = 0; i < MAX_SUBSCRIBERS; i++)
Gary Servin 0:04ac6be8229a 497 {
Gary Servin 0:04ac6be8229a 498 if (subscribers[i] != 0) // non-empty slot
Gary Servin 0:04ac6be8229a 499 {
Gary Servin 0:04ac6be8229a 500 ti.topic_id = subscribers[i]->id_;
Gary Servin 0:04ac6be8229a 501 ti.topic_name = (char *) subscribers[i]->topic_;
Gary Servin 0:04ac6be8229a 502 ti.message_type = (char *) subscribers[i]->getMsgType();
Gary Servin 0:04ac6be8229a 503 ti.md5sum = (char *) subscribers[i]->getMsgMD5();
Gary Servin 0:04ac6be8229a 504 ti.buffer_size = INPUT_SIZE;
Gary Servin 0:04ac6be8229a 505 publish(subscribers[i]->getEndpointType(), &ti);
Gary Servin 0:04ac6be8229a 506 }
Gary Servin 0:04ac6be8229a 507 }
Gary Servin 0:04ac6be8229a 508 configured_ = true;
Gary Servin 0:04ac6be8229a 509 }
Gary Servin 0:04ac6be8229a 510
Gary Servin 0:04ac6be8229a 511 virtual int publish(int id, const Msg * msg)
Gary Servin 0:04ac6be8229a 512 {
Gary Servin 0:04ac6be8229a 513 if (id >= 100 && !configured_)
Gary Servin 0:04ac6be8229a 514 return 0;
Gary Servin 0:04ac6be8229a 515
Gary Servin 0:04ac6be8229a 516 /* serialize message */
Gary Servin 0:04ac6be8229a 517 int l = msg->serialize(message_out + 7);
Gary Servin 0:04ac6be8229a 518
Gary Servin 0:04ac6be8229a 519 /* setup the header */
Gary Servin 0:04ac6be8229a 520 message_out[0] = 0xff;
Gary Servin 0:04ac6be8229a 521 message_out[1] = PROTOCOL_VER;
Gary Servin 0:04ac6be8229a 522 message_out[2] = (uint8_t)((uint16_t)l & 255);
Gary Servin 0:04ac6be8229a 523 message_out[3] = (uint8_t)((uint16_t)l >> 8);
Gary Servin 0:04ac6be8229a 524 message_out[4] = 255 - ((message_out[2] + message_out[3]) % 256);
Gary Servin 0:04ac6be8229a 525 message_out[5] = (uint8_t)((int16_t)id & 255);
Gary Servin 0:04ac6be8229a 526 message_out[6] = (uint8_t)((int16_t)id >> 8);
Gary Servin 0:04ac6be8229a 527
Gary Servin 0:04ac6be8229a 528 /* calculate checksum */
Gary Servin 0:04ac6be8229a 529 int chk = 0;
Gary Servin 0:04ac6be8229a 530 for (int i = 5; i < l + 7; i++)
Gary Servin 0:04ac6be8229a 531 chk += message_out[i];
Gary Servin 0:04ac6be8229a 532 l += 7;
Gary Servin 0:04ac6be8229a 533 message_out[l++] = 255 - (chk % 256);
Gary Servin 0:04ac6be8229a 534
Gary Servin 0:04ac6be8229a 535 if (l <= OUTPUT_SIZE)
Gary Servin 0:04ac6be8229a 536 {
Gary Servin 0:04ac6be8229a 537 hardware_.write(message_out, l);
Gary Servin 0:04ac6be8229a 538 return l;
Gary Servin 0:04ac6be8229a 539 }
Gary Servin 0:04ac6be8229a 540 else
Gary Servin 0:04ac6be8229a 541 {
Gary Servin 0:04ac6be8229a 542 logerror("Message from device dropped: message larger than buffer.");
Gary Servin 0:04ac6be8229a 543 return -1;
Gary Servin 0:04ac6be8229a 544 }
Gary Servin 0:04ac6be8229a 545 }
Gary Servin 0:04ac6be8229a 546
Gary Servin 0:04ac6be8229a 547 /********************************************************************
Gary Servin 0:04ac6be8229a 548 * Logging
Gary Servin 0:04ac6be8229a 549 */
Gary Servin 0:04ac6be8229a 550
Gary Servin 0:04ac6be8229a 551 protected:
Gary Servin 0:04ac6be8229a 552 void log(char byte, const char * msg)
Gary Servin 0:04ac6be8229a 553 {
Gary Servin 0:04ac6be8229a 554 rosserial_msgs::Log l;
Gary Servin 0:04ac6be8229a 555 l.level = byte;
Gary Servin 0:04ac6be8229a 556 l.msg = (char*)msg;
Gary Servin 0:04ac6be8229a 557 publish(rosserial_msgs::TopicInfo::ID_LOG, &l);
Gary Servin 0:04ac6be8229a 558 }
Gary Servin 0:04ac6be8229a 559
Gary Servin 0:04ac6be8229a 560 public:
Gary Servin 0:04ac6be8229a 561 void logdebug(const char* msg)
Gary Servin 0:04ac6be8229a 562 {
Gary Servin 0:04ac6be8229a 563 log(rosserial_msgs::Log::ROSDEBUG, msg);
Gary Servin 0:04ac6be8229a 564 }
Gary Servin 0:04ac6be8229a 565 void loginfo(const char * msg)
Gary Servin 0:04ac6be8229a 566 {
Gary Servin 0:04ac6be8229a 567 log(rosserial_msgs::Log::INFO, msg);
Gary Servin 0:04ac6be8229a 568 }
Gary Servin 0:04ac6be8229a 569 void logwarn(const char *msg)
Gary Servin 0:04ac6be8229a 570 {
Gary Servin 0:04ac6be8229a 571 log(rosserial_msgs::Log::WARN, msg);
Gary Servin 0:04ac6be8229a 572 }
Gary Servin 0:04ac6be8229a 573 void logerror(const char*msg)
Gary Servin 0:04ac6be8229a 574 {
Gary Servin 0:04ac6be8229a 575 log(rosserial_msgs::Log::ERROR, msg);
Gary Servin 0:04ac6be8229a 576 }
Gary Servin 0:04ac6be8229a 577 void logfatal(const char*msg)
Gary Servin 0:04ac6be8229a 578 {
Gary Servin 0:04ac6be8229a 579 log(rosserial_msgs::Log::FATAL, msg);
Gary Servin 0:04ac6be8229a 580 }
Gary Servin 0:04ac6be8229a 581
Gary Servin 0:04ac6be8229a 582 /********************************************************************
Gary Servin 0:04ac6be8229a 583 * Parameters
Gary Servin 0:04ac6be8229a 584 */
Gary Servin 0:04ac6be8229a 585
Gary Servin 0:04ac6be8229a 586 protected:
Gary Servin 0:04ac6be8229a 587 bool param_recieved;
Gary Servin 0:04ac6be8229a 588 rosserial_msgs::RequestParamResponse req_param_resp;
Gary Servin 0:04ac6be8229a 589
Gary Servin 0:04ac6be8229a 590 bool requestParam(const char * name, int time_out = 1000)
Gary Servin 0:04ac6be8229a 591 {
Gary Servin 0:04ac6be8229a 592 param_recieved = false;
Gary Servin 0:04ac6be8229a 593 rosserial_msgs::RequestParamRequest req;
Gary Servin 0:04ac6be8229a 594 req.name = (char*)name;
Gary Servin 0:04ac6be8229a 595 publish(TopicInfo::ID_PARAMETER_REQUEST, &req);
Gary Servin 0:04ac6be8229a 596 uint32_t end_time = hardware_.time() + time_out;
Gary Servin 0:04ac6be8229a 597 while (!param_recieved)
Gary Servin 0:04ac6be8229a 598 {
Gary Servin 0:04ac6be8229a 599 spinOnce();
Gary Servin 0:04ac6be8229a 600 if (hardware_.time() > end_time)
Gary Servin 0:04ac6be8229a 601 {
Gary Servin 0:04ac6be8229a 602 logwarn("Failed to get param: timeout expired");
Gary Servin 0:04ac6be8229a 603 return false;
Gary Servin 0:04ac6be8229a 604 }
Gary Servin 0:04ac6be8229a 605 }
Gary Servin 0:04ac6be8229a 606 return true;
Gary Servin 0:04ac6be8229a 607 }
Gary Servin 0:04ac6be8229a 608
Gary Servin 0:04ac6be8229a 609 public:
Gary Servin 0:04ac6be8229a 610 bool getParam(const char* name, int* param, int length = 1, int timeout = 1000)
Gary Servin 0:04ac6be8229a 611 {
Gary Servin 0:04ac6be8229a 612 if (requestParam(name, timeout))
Gary Servin 0:04ac6be8229a 613 {
Gary Servin 0:04ac6be8229a 614 if (length == req_param_resp.ints_length)
Gary Servin 0:04ac6be8229a 615 {
Gary Servin 0:04ac6be8229a 616 //copy it over
Gary Servin 0:04ac6be8229a 617 for (int i = 0; i < length; i++)
Gary Servin 0:04ac6be8229a 618 param[i] = req_param_resp.ints[i];
Gary Servin 0:04ac6be8229a 619 return true;
Gary Servin 0:04ac6be8229a 620 }
Gary Servin 0:04ac6be8229a 621 else
Gary Servin 0:04ac6be8229a 622 {
Gary Servin 0:04ac6be8229a 623 logwarn("Failed to get param: length mismatch");
Gary Servin 0:04ac6be8229a 624 }
Gary Servin 0:04ac6be8229a 625 }
Gary Servin 0:04ac6be8229a 626 return false;
Gary Servin 0:04ac6be8229a 627 }
Gary Servin 0:04ac6be8229a 628 bool getParam(const char* name, float* param, int length = 1, int timeout = 1000)
Gary Servin 0:04ac6be8229a 629 {
Gary Servin 0:04ac6be8229a 630 if (requestParam(name, timeout))
Gary Servin 0:04ac6be8229a 631 {
Gary Servin 0:04ac6be8229a 632 if (length == req_param_resp.floats_length)
Gary Servin 0:04ac6be8229a 633 {
Gary Servin 0:04ac6be8229a 634 //copy it over
Gary Servin 0:04ac6be8229a 635 for (int i = 0; i < length; i++)
Gary Servin 0:04ac6be8229a 636 param[i] = req_param_resp.floats[i];
Gary Servin 0:04ac6be8229a 637 return true;
Gary Servin 0:04ac6be8229a 638 }
Gary Servin 0:04ac6be8229a 639 else
Gary Servin 0:04ac6be8229a 640 {
Gary Servin 0:04ac6be8229a 641 logwarn("Failed to get param: length mismatch");
Gary Servin 0:04ac6be8229a 642 }
Gary Servin 0:04ac6be8229a 643 }
Gary Servin 0:04ac6be8229a 644 return false;
Gary Servin 0:04ac6be8229a 645 }
Gary Servin 0:04ac6be8229a 646 bool getParam(const char* name, char** param, int length = 1, int timeout = 1000)
Gary Servin 0:04ac6be8229a 647 {
Gary Servin 0:04ac6be8229a 648 if (requestParam(name, timeout))
Gary Servin 0:04ac6be8229a 649 {
Gary Servin 0:04ac6be8229a 650 if (length == req_param_resp.strings_length)
Gary Servin 0:04ac6be8229a 651 {
Gary Servin 0:04ac6be8229a 652 //copy it over
Gary Servin 0:04ac6be8229a 653 for (int i = 0; i < length; i++)
Gary Servin 0:04ac6be8229a 654 strcpy(param[i], req_param_resp.strings[i]);
Gary Servin 0:04ac6be8229a 655 return true;
Gary Servin 0:04ac6be8229a 656 }
Gary Servin 0:04ac6be8229a 657 else
Gary Servin 0:04ac6be8229a 658 {
Gary Servin 0:04ac6be8229a 659 logwarn("Failed to get param: length mismatch");
Gary Servin 0:04ac6be8229a 660 }
Gary Servin 0:04ac6be8229a 661 }
Gary Servin 0:04ac6be8229a 662 return false;
Gary Servin 0:04ac6be8229a 663 }
Gary Servin 0:04ac6be8229a 664 bool getParam(const char* name, bool* param, int length = 1, int timeout = 1000)
Gary Servin 0:04ac6be8229a 665 {
Gary Servin 0:04ac6be8229a 666 if (requestParam(name, timeout))
Gary Servin 0:04ac6be8229a 667 {
Gary Servin 0:04ac6be8229a 668 if (length == req_param_resp.ints_length)
Gary Servin 0:04ac6be8229a 669 {
Gary Servin 0:04ac6be8229a 670 //copy it over
Gary Servin 0:04ac6be8229a 671 for (int i = 0; i < length; i++)
Gary Servin 0:04ac6be8229a 672 param[i] = req_param_resp.ints[i];
Gary Servin 0:04ac6be8229a 673 return true;
Gary Servin 0:04ac6be8229a 674 }
Gary Servin 0:04ac6be8229a 675 else
Gary Servin 0:04ac6be8229a 676 {
Gary Servin 0:04ac6be8229a 677 logwarn("Failed to get param: length mismatch");
Gary Servin 0:04ac6be8229a 678 }
Gary Servin 0:04ac6be8229a 679 }
Gary Servin 0:04ac6be8229a 680 return false;
Gary Servin 0:04ac6be8229a 681 }
Gary Servin 0:04ac6be8229a 682 };
Gary Servin 0:04ac6be8229a 683
Gary Servin 0:04ac6be8229a 684 }
Gary Servin 0:04ac6be8229a 685
Gary Servin 0:04ac6be8229a 686 #endif