La Suno / Mbed 2 deprecated afero_poc15_180223

Color sensor reset at the end of calibration added. sensor id auto assignment was changed to be a fixed value assignment to avoid sensor id shift when some sensor is absent.

Dependencies:   UniGraphic mbed vt100

afLib/Command.cpp@1:8818b793d147, 2018-02-23 (annotated)

Committer:
Rhyme
Date:
Fri Feb 23 07:51:55 2018 +0000
Revision:
1:8818b793d147
Parent:
0:ce97f6d34336 
Wrong behavior when one of color sensor is missing has been fixed.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhyme 0:ce97f6d34336 1 /**
Rhyme 0:ce97f6d34336 2 * Copyright 2015 Afero, Inc.
Rhyme 0:ce97f6d34336 3 *
Rhyme 0:ce97f6d34336 4 * Licensed under the Apache License, Version 2.0 (the "License");
Rhyme 0:ce97f6d34336 5 * you may not use this file except in compliance with the License.
Rhyme 0:ce97f6d34336 6 * You may obtain a copy of the License at
Rhyme 0:ce97f6d34336 7 *
Rhyme 0:ce97f6d34336 8 * http://www.apache.org/licenses/LICENSE-2.0
Rhyme 0:ce97f6d34336 9 *
Rhyme 0:ce97f6d34336 10 * Unless required by applicable law or agreed to in writing, software
Rhyme 0:ce97f6d34336 11 * distributed under the License is distributed on an "AS IS" BASIS,
Rhyme 0:ce97f6d34336 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
Rhyme 0:ce97f6d34336 13 * See the License for the specific language governing permissions and
Rhyme 0:ce97f6d34336 14 * limitations under the License.
Rhyme 0:ce97f6d34336 15 */
Rhyme 0:ce97f6d34336 16
Rhyme 0:ce97f6d34336 17 //wsugi #include "Arduino.h"
Rhyme 0:ce97f6d34336 18 #include "mbed.h"
Rhyme 0:ce97f6d34336 19 #include <stdio.h>
Rhyme 0:ce97f6d34336 20 #include "Command.h"
Rhyme 0:ce97f6d34336 21 #include "msg_types.h"
Rhyme 0:ce97f6d34336 22 #include "DebugIO.hpp"
Rhyme 0:ce97f6d34336 23
Rhyme 0:ce97f6d34336 24 #define CMD_HDR_LEN 4 // 4 byte header on all commands
Rhyme 0:ce97f6d34336 25 #define CMD_VAL_LEN 2 // 2 byte value length for commands that have a value
Rhyme 0:ce97f6d34336 26
Rhyme 0:ce97f6d34336 27 const char *CMD_NAMES[] = {"SET ", "GET ", "UPDATE"};
Rhyme 0:ce97f6d34336 28
Rhyme 0:ce97f6d34336 29
Rhyme 0:ce97f6d34336 30 Command::Command(uint16_t len, uint8_t *bytes) {
Rhyme 0:ce97f6d34336 31 int index = 0;
Rhyme 0:ce97f6d34336 32
Rhyme 0:ce97f6d34336 33 _cmd = bytes[index++];
Rhyme 0:ce97f6d34336 34 _requestId = bytes[index++];
Rhyme 0:ce97f6d34336 35 _attrId = bytes[index + 0] | bytes[index + 1] << 8;
Rhyme 0:ce97f6d34336 36 index += 2;
Rhyme 0:ce97f6d34336 37
Rhyme 0:ce97f6d34336 38 if (_cmd == MSG_TYPE_GET) {
Rhyme 0:ce97f6d34336 39 return;
Rhyme 0:ce97f6d34336 40 }
Rhyme 0:ce97f6d34336 41 if (_cmd == MSG_TYPE_UPDATE) {
Rhyme 0:ce97f6d34336 42 _status = bytes[index++];
Rhyme 0:ce97f6d34336 43 _reason = bytes[index++];
Rhyme 0:ce97f6d34336 44 }
Rhyme 0:ce97f6d34336 45
Rhyme 0:ce97f6d34336 46 _valueLen = bytes[index + 0] | bytes[index + 1] << 8;
Rhyme 0:ce97f6d34336 47 index += 2;
Rhyme 0:ce97f6d34336 48 _value = new uint8_t[_valueLen];
Rhyme 0:ce97f6d34336 49 for (int i = 0; i < _valueLen; i++) {
Rhyme 0:ce97f6d34336 50 _value[i] = bytes[index + i];
Rhyme 0:ce97f6d34336 51 }
Rhyme 0:ce97f6d34336 52 }
Rhyme 0:ce97f6d34336 53
Rhyme 0:ce97f6d34336 54 Command::Command(uint8_t requestId, const char *str) {
Rhyme 0:ce97f6d34336 55 _requestId = requestId & 0xff;
Rhyme 0:ce97f6d34336 56
Rhyme 0:ce97f6d34336 57 char *cp; //wsugi = strdup(str);
Rhyme 0:ce97f6d34336 58 //wsugi 00 start
Rhyme 0:ce97f6d34336 59 {
Rhyme 0:ce97f6d34336 60 int length = strlen(str)+1;
Rhyme 0:ce97f6d34336 61 cp = (char*)malloc(length);
Rhyme 0:ce97f6d34336 62 strcpy(cp,str);
Rhyme 0:ce97f6d34336 63 }
Rhyme 0:ce97f6d34336 64 //wsugi 00 end
Rhyme 0:ce97f6d34336 65 char *tok = strtok(cp, " ");
Rhyme 0:ce97f6d34336 66 _cmd = strToCmd(tok);
Rhyme 0:ce97f6d34336 67
Rhyme 0:ce97f6d34336 68 tok = strtok(NULL, " ");
Rhyme 0:ce97f6d34336 69 _attrId = strToAttrId(tok);
Rhyme 0:ce97f6d34336 70
Rhyme 0:ce97f6d34336 71 if (_cmd == MSG_TYPE_GET) {
Rhyme 0:ce97f6d34336 72 _valueLen = 0;
Rhyme 0:ce97f6d34336 73 _value = NULL;
Rhyme 0:ce97f6d34336 74 } else {
Rhyme 0:ce97f6d34336 75 tok = strtok(NULL, " ");
Rhyme 0:ce97f6d34336 76 _valueLen = strlen(tok) / 2;
Rhyme 0:ce97f6d34336 77 _value = new uint8_t[_valueLen];
Rhyme 0:ce97f6d34336 78 strToValue(tok, _value);
Rhyme 0:ce97f6d34336 79 }
Rhyme 0:ce97f6d34336 80
Rhyme 0:ce97f6d34336 81 free(cp);
Rhyme 0:ce97f6d34336 82 }
Rhyme 0:ce97f6d34336 83
Rhyme 0:ce97f6d34336 84 Command::Command(uint8_t requestId, uint8_t cmd, uint16_t attrId) {
Rhyme 0:ce97f6d34336 85 _requestId = requestId;
Rhyme 0:ce97f6d34336 86 _cmd = cmd;
Rhyme 0:ce97f6d34336 87 _attrId = attrId;
Rhyme 0:ce97f6d34336 88 _valueLen = 0;
Rhyme 0:ce97f6d34336 89 _value = NULL;
Rhyme 0:ce97f6d34336 90 }
Rhyme 0:ce97f6d34336 91
Rhyme 0:ce97f6d34336 92 Command::Command(uint8_t requestId, uint8_t cmd, uint16_t attrId, uint16_t valueLen, uint8_t *value) {
Rhyme 0:ce97f6d34336 93 _requestId = requestId;
Rhyme 0:ce97f6d34336 94 _cmd = cmd;
Rhyme 0:ce97f6d34336 95 _attrId = attrId;
Rhyme 0:ce97f6d34336 96 _valueLen = valueLen;
Rhyme 0:ce97f6d34336 97 _value = new uint8_t[_valueLen];
Rhyme 0:ce97f6d34336 98 memcpy(_value, value, valueLen);
Rhyme 0:ce97f6d34336 99 }
Rhyme 0:ce97f6d34336 100
Rhyme 0:ce97f6d34336 101 Command::Command(uint8_t requestId, uint8_t cmd, uint16_t attrId, uint8_t status, uint8_t reason, uint16_t valueLen,
Rhyme 0:ce97f6d34336 102 uint8_t *value) {
Rhyme 0:ce97f6d34336 103 _requestId = requestId;
Rhyme 0:ce97f6d34336 104 _cmd = cmd;
Rhyme 0:ce97f6d34336 105 _attrId = attrId;
Rhyme 0:ce97f6d34336 106 _status = status;
Rhyme 0:ce97f6d34336 107 _reason = reason;
Rhyme 0:ce97f6d34336 108 _valueLen = valueLen;
Rhyme 0:ce97f6d34336 109 _value = new uint8_t[_valueLen];
Rhyme 0:ce97f6d34336 110 memcpy(_value, value, valueLen);
Rhyme 0:ce97f6d34336 111 }
Rhyme 0:ce97f6d34336 112
Rhyme 0:ce97f6d34336 113 Command::Command() {
Rhyme 0:ce97f6d34336 114 }
Rhyme 0:ce97f6d34336 115
Rhyme 0:ce97f6d34336 116 Command::~Command() {
Rhyme 0:ce97f6d34336 117 if (_value != NULL) {
Rhyme 0:ce97f6d34336 118 delete[] _value; //wsugi delete (_value);
Rhyme 0:ce97f6d34336 119 }
Rhyme 0:ce97f6d34336 120 }
Rhyme 0:ce97f6d34336 121
Rhyme 0:ce97f6d34336 122 int Command::strToValue(char *valueStr, uint8_t *value) {
Rhyme 0:ce97f6d34336 123 for (int i = 0; i < (int) (strlen(valueStr) / 2); i++) {
Rhyme 0:ce97f6d34336 124 int j = i * 2;
Rhyme 0:ce97f6d34336 125 value[i] = getVal(valueStr[j + 1]) + (getVal(valueStr[j]) << 4);
Rhyme 0:ce97f6d34336 126 }
Rhyme 0:ce97f6d34336 127
Rhyme 0:ce97f6d34336 128 return 0;
Rhyme 0:ce97f6d34336 129 }
Rhyme 0:ce97f6d34336 130
Rhyme 0:ce97f6d34336 131 uint16_t Command::strToAttrId(char *attrIdStr) {
Rhyme 0:ce97f6d34336 132 return atoi(attrIdStr);
Rhyme 0:ce97f6d34336 133 //return String(attrIdStr).toInt();
Rhyme 0:ce97f6d34336 134 }
Rhyme 0:ce97f6d34336 135
Rhyme 0:ce97f6d34336 136 uint8_t Command::strToCmd(char *cmdStr) {
Rhyme 0:ce97f6d34336 137 char c = cmdStr[0];
Rhyme 0:ce97f6d34336 138 if (c == 'g' || c == 'G') {
Rhyme 0:ce97f6d34336 139 return MSG_TYPE_GET;
Rhyme 0:ce97f6d34336 140 } else if (c == 's' || c == 'S') {
Rhyme 0:ce97f6d34336 141 return MSG_TYPE_SET;
Rhyme 0:ce97f6d34336 142 } else if (c == 'u' || c == 'U') {
Rhyme 0:ce97f6d34336 143 return MSG_TYPE_UPDATE;
Rhyme 0:ce97f6d34336 144 }
Rhyme 0:ce97f6d34336 145
Rhyme 0:ce97f6d34336 146 return 0xFF ;
Rhyme 0:ce97f6d34336 147 }
Rhyme 0:ce97f6d34336 148
Rhyme 0:ce97f6d34336 149 uint8_t Command::getCommand() {
Rhyme 0:ce97f6d34336 150 return _cmd;
Rhyme 0:ce97f6d34336 151 }
Rhyme 0:ce97f6d34336 152
Rhyme 0:ce97f6d34336 153 uint8_t Command::getReqId() {
Rhyme 0:ce97f6d34336 154 return _requestId;
Rhyme 0:ce97f6d34336 155 }
Rhyme 0:ce97f6d34336 156
Rhyme 0:ce97f6d34336 157 uint16_t Command::getAttrId() {
Rhyme 0:ce97f6d34336 158 return _attrId;
Rhyme 0:ce97f6d34336 159 }
Rhyme 0:ce97f6d34336 160
Rhyme 0:ce97f6d34336 161 uint16_t Command::getValueLen() {
Rhyme 0:ce97f6d34336 162 return _valueLen;
Rhyme 0:ce97f6d34336 163 }
Rhyme 0:ce97f6d34336 164
Rhyme 0:ce97f6d34336 165 void Command::getValue(uint8_t *value) {
Rhyme 0:ce97f6d34336 166 for (int i = 0; i < _valueLen; i++) {
Rhyme 0:ce97f6d34336 167 value[i] = _value[i];
Rhyme 0:ce97f6d34336 168 }
Rhyme 0:ce97f6d34336 169 }
Rhyme 0:ce97f6d34336 170
Rhyme 0:ce97f6d34336 171 uint8_t *Command::getValueP() {
Rhyme 0:ce97f6d34336 172 return _value;
Rhyme 0:ce97f6d34336 173 }
Rhyme 0:ce97f6d34336 174
Rhyme 0:ce97f6d34336 175 uint16_t Command::getSize() {
Rhyme 0:ce97f6d34336 176 uint16_t len = CMD_HDR_LEN;
Rhyme 0:ce97f6d34336 177
Rhyme 0:ce97f6d34336 178 if (_cmd != MSG_TYPE_GET) {
Rhyme 0:ce97f6d34336 179 len += CMD_VAL_LEN + _valueLen;
Rhyme 0:ce97f6d34336 180 }
Rhyme 0:ce97f6d34336 181
Rhyme 0:ce97f6d34336 182 if (_cmd == MSG_TYPE_UPDATE) {
Rhyme 0:ce97f6d34336 183 len += 2; // status byte + reason byte
Rhyme 0:ce97f6d34336 184 }
Rhyme 0:ce97f6d34336 185
Rhyme 0:ce97f6d34336 186 return len;
Rhyme 0:ce97f6d34336 187 }
Rhyme 0:ce97f6d34336 188
Rhyme 0:ce97f6d34336 189 uint16_t Command::getBytes(uint8_t *bytes) {
Rhyme 0:ce97f6d34336 190 uint16_t len = getSize();
Rhyme 0:ce97f6d34336 191
Rhyme 0:ce97f6d34336 192 int index = 0;
Rhyme 0:ce97f6d34336 193
Rhyme 0:ce97f6d34336 194 bytes[index++] = (_cmd);
Rhyme 0:ce97f6d34336 195
Rhyme 0:ce97f6d34336 196 bytes[index++] = (_requestId);
Rhyme 0:ce97f6d34336 197
Rhyme 0:ce97f6d34336 198 bytes[index++] = (_attrId & 0xff);
Rhyme 0:ce97f6d34336 199 bytes[index++] = ((_attrId >> 8) & 0xff);
Rhyme 0:ce97f6d34336 200
Rhyme 0:ce97f6d34336 201 if (_cmd == MSG_TYPE_GET) {
Rhyme 0:ce97f6d34336 202 return len;
Rhyme 0:ce97f6d34336 203 }
Rhyme 0:ce97f6d34336 204
Rhyme 0:ce97f6d34336 205 if (_cmd == MSG_TYPE_UPDATE) {
Rhyme 0:ce97f6d34336 206 bytes[index++] = (_status);
Rhyme 0:ce97f6d34336 207 bytes[index++] = (_reason);
Rhyme 0:ce97f6d34336 208 }
Rhyme 0:ce97f6d34336 209
Rhyme 0:ce97f6d34336 210 bytes[index++] = (_valueLen & 0xff);
Rhyme 0:ce97f6d34336 211 bytes[index++] = ((_valueLen >> 8) & 0xff);
Rhyme 0:ce97f6d34336 212
Rhyme 0:ce97f6d34336 213 for (int i = 0; i < _valueLen; i++) {
Rhyme 0:ce97f6d34336 214 bytes[index++] = (_value[i]);
Rhyme 0:ce97f6d34336 215 }
Rhyme 0:ce97f6d34336 216
Rhyme 0:ce97f6d34336 217 return len;
Rhyme 0:ce97f6d34336 218 }
Rhyme 0:ce97f6d34336 219
Rhyme 0:ce97f6d34336 220 bool Command::isValid() {
Rhyme 0:ce97f6d34336 221 return (_cmd == MSG_TYPE_SET) || (_cmd == MSG_TYPE_GET) || (_cmd == MSG_TYPE_UPDATE);
Rhyme 0:ce97f6d34336 222 }
Rhyme 0:ce97f6d34336 223
Rhyme 0:ce97f6d34336 224 void Command::dumpBytes() {
Rhyme 0:ce97f6d34336 225 uint16_t len = getSize();
Rhyme 0:ce97f6d34336 226 uint8_t bytes[len];
Rhyme 0:ce97f6d34336 227 getBytes(bytes);
Rhyme 0:ce97f6d34336 228
Rhyme 0:ce97f6d34336 229 _printBuf[0] = 0;
Rhyme 0:ce97f6d34336 230 sprintf(_printBuf, "len: %d value: ", len);
Rhyme 0:ce97f6d34336 231 for (int i = 0; i < len; i++) {
Rhyme 0:ce97f6d34336 232 int b = bytes[i] & 0xff;
Rhyme 0:ce97f6d34336 233 sprintf(&_printBuf[strlen(_printBuf)], "%02x", b);
Rhyme 0:ce97f6d34336 234 }
Rhyme 0:ce97f6d34336 235 SERIAL_PRINT_DBG_ASR("%s\n",_printBuf);
Rhyme 0:ce97f6d34336 236 }
Rhyme 0:ce97f6d34336 237
Rhyme 0:ce97f6d34336 238 void Command::dump() {
Rhyme 0:ce97f6d34336 239 _printBuf[0] = 0;
Rhyme 0:ce97f6d34336 240 sprintf(_printBuf, "cmd: %s attr: %d value: ",
Rhyme 0:ce97f6d34336 241 CMD_NAMES[_cmd - MESSAGE_CHANNEL_BASE - 1],
Rhyme 0:ce97f6d34336 242 _attrId
Rhyme 0:ce97f6d34336 243 );
Rhyme 0:ce97f6d34336 244 if (_cmd != MSG_TYPE_GET) {
Rhyme 0:ce97f6d34336 245 for (int i = 0; i < _valueLen; i++) {
Rhyme 0:ce97f6d34336 246 int b = _value[i] & 0xff;
Rhyme 0:ce97f6d34336 247 sprintf(&_printBuf[strlen(_printBuf)], "%02x", b);
Rhyme 0:ce97f6d34336 248 }
Rhyme 0:ce97f6d34336 249 }
Rhyme 0:ce97f6d34336 250 SERIAL_PRINT_DBG_ASR("%s\n",_printBuf);
Rhyme 0:ce97f6d34336 251 }
Rhyme 0:ce97f6d34336 252
Rhyme 0:ce97f6d34336 253 uint8_t Command::getVal(char c) {
Rhyme 0:ce97f6d34336 254 if (c >= '0' && c <= '9')
Rhyme 0:ce97f6d34336 255 return (uint8_t)(c - '0');
Rhyme 0:ce97f6d34336 256 else if (c >= 'A' && c <= 'F')
Rhyme 0:ce97f6d34336 257 return (uint8_t)(c - 'A' + 10);
Rhyme 0:ce97f6d34336 258 else if (c >= 'a' && c <= 'f')
Rhyme 0:ce97f6d34336 259 return (uint8_t)(c - 'a' + 10);
Rhyme 0:ce97f6d34336 260
Rhyme 0:ce97f6d34336 261 SERIAL_PRINT_DBG_ASR("bad hex char: %c\n",c);
Rhyme 0:ce97f6d34336 262
Rhyme 0:ce97f6d34336 263 return 0;
Rhyme 0:ce97f6d34336 264 }