You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
137 lines
8.2 KiB
137 lines
8.2 KiB
/**
|
|
* \file
|
|
* \brief Module to identify different devices based on their unique ID.
|
|
* \author Erich Styger, erich.styger@hslu.ch
|
|
* \license SPDX-License-Identifier: BSD-3-Clause
|
|
* With this module individual devices are identified based on their unique ID.
|
|
*/
|
|
|
|
#include "platform.h"
|
|
#if PL_CONFIG_USE_IDENTIFY
|
|
#include "Identify.h"
|
|
#include "McuArmTools.h"
|
|
#include "McuUtility.h"
|
|
|
|
static ID_Robot_e currID = ID_ROBOT_NONE;
|
|
|
|
typedef struct {
|
|
ID_Robot_e id;
|
|
const unsigned char *name;
|
|
McuArmTools_UID uuid; /* UID of K22 */
|
|
} ID_RobotDevice;
|
|
|
|
static const ID_RobotDevice idTable[] =
|
|
{ /*! \todo ADIS: Verify and update information */
|
|
{.id=ID_ROBOT_E0, .name=(const unsigned char*)"Robot E0", .uuid={ {0x00,0x00,0x34,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E1, .name=(const unsigned char*)"Robot E1", .uuid={ {0x00,0x00,0x38,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E2, .name=(const unsigned char*)"Robot E2", .uuid={ {0x00,0x00,0x33,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E3, .name=(const unsigned char*)"Robot E3", .uuid={ {0x00,0x00,0x13,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E4, .name=(const unsigned char*)"Robot E4", .uuid={ {0x00,0x00,0x34,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E5, .name=(const unsigned char*)"Robot E5", .uuid={ {0x00,0x00,0x00,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E6, .name=(const unsigned char*)"Robot E6", .uuid={ {0xFF,0xFF,0x10,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_E7, .name=(const unsigned char*)"Robot E7", .uuid={ {0x00,0x00,0x37,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E8, .name=(const unsigned char*)"Robot E8", .uuid={ {0x00,0x00,0x19,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E9, .name=(const unsigned char*)"Robot E9", .uuid={ {0xFF,0xFF,0x0B,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x24,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_E10, .name=(const unsigned char*)"Robot E10", .uuid={ {0xFF,0xFF,0x20,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x07,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_E11, .name=(const unsigned char*)"Robot E11", .uuid={ {0x00,0x00,0x1F,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E12, .name=(const unsigned char*)"Robot E12", .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_E13, .name=(const unsigned char*)"Robot E13", .uuid={ {0x00,0x00,0x0D,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E14, .name=(const unsigned char*)"Robot E14", .uuid={ {0x00,0x00,0x0C,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
|
|
{.id=ID_ROBOT_E17, .name=(const unsigned char*)"Robot E17", .uuid={ {0x00,0x00,0x2C,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_E18, .name=(const unsigned char*)"Robot E18", .uuid={ {0xFF,0xFF,0x1E,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x07,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_E27, .name=(const unsigned char*)"Robot E27", .uuid={ {0x00,0x00,0x36,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
|
|
{.id=ID_ROBOT_L0, .name=(const unsigned char*)"Robot L0", .uuid={ {0x00,0x00,0x1B,0x00,0x01,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_L1, .name=(const unsigned char*)"Robot L1", .uuid={ {0xFF,0xFF,0x19,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_L3, .name=(const unsigned char*)"Robot L3", .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x0A,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_L17, .name=(const unsigned char*)"Robot L17", .uuid={ {0x00,0x00,0x2F,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_L20, .name=(const unsigned char*)"Robot L20", .uuid={ {0x00,0x00,0x0A,0x00,0x21,0xB8,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
|
|
{.id=ID_ROBOT_R0, .name=(const unsigned char*)"Robot R8", .uuid={ {0x00,0x00,0x03,0x00,0x21,0xB7,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R8, .name=(const unsigned char*)"Robot R8", .uuid={ {0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x25,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_R9, .name=(const unsigned char*)"Robot R9", .uuid={ {0xFF,0xFF,0x03,0x00,0xFF,0xFF,0xFF,0xFF,0x99,0x27,0x45,0x4E,0x24,0x00,0x02,0x10} }},
|
|
{.id=ID_ROBOT_R23, .name=(const unsigned char*)"Robot R23", .uuid={ {0x00,0x00,0x29,0x00,0x51,0xB4,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R27, .name=(const unsigned char*)"Robot R27", .uuid={ {0x00,0x00,0x30,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R28, .name=(const unsigned char*)"Robot R28", .uuid={ {0x00,0x00,0x39,0x00,0x21,0xB8,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R29, .name=(const unsigned char*)"Robot R29", .uuid={ {0x00,0x00,0x00,0x00,0x01,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R32, .name=(const unsigned char*)"Robot R32", .uuid={ {0x00,0x00,0x00,0x00,0x11,0xB9,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R33, .name=(const unsigned char*)"Robot R33", .uuid={ {0x00,0x00,0x37,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R34, .name=(const unsigned char*)"Robot R34", .uuid={ {0x00,0x00,0x35,0x00,0x31,0xB6,0xCD,0x67,0x15,0x32,0x45,0x4E,0x14,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R36, .name=(const unsigned char*)"Robot R36", .uuid={ {0x00,0x00,0x3A,0x00,0x41,0xB5,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R37, .name=(const unsigned char*)"Robot R37", .uuid={ {0x00,0x00,0x1C,0x00,0xF1,0xBA,0xCD,0x67,0x15,0x32,0x45,0x4E,0x13,0x00,0x02,0x30} }},
|
|
{.id=ID_ROBOT_R44, .name=(const unsigned char*)"Robot R44", .uuid={ {0x00,0x00,0x2F,0x00,0x41,0x6A,0xD2,0x67,0x15,0x32,0x45,0x4E,0x32,0x00,0x02,0x30} }},
|
|
};
|
|
|
|
static ID_Robot_e IdentifyDevice(void) {
|
|
uint8_t res;
|
|
McuArmTools_UID id;
|
|
unsigned int i;
|
|
|
|
res = McuArmTools_UIDGet(&id);
|
|
if (res==ERR_OK) {
|
|
for(i=0; i<sizeof(idTable)/sizeof(idTable[0]); i++) {
|
|
if (McuArmTools_UIDSame(&id, &idTable[i].uuid)) {
|
|
return idTable[i].id; /* found it */
|
|
}
|
|
}
|
|
}
|
|
return ID_ROBOT_UNKNOWN;
|
|
}
|
|
|
|
static const ID_RobotDevice *GetDeviceDesc(ID_Robot_e id) {
|
|
unsigned int i;
|
|
|
|
for(i=0; i<sizeof(idTable)/sizeof(idTable[0]); i++) {
|
|
if (idTable[i].id == id) {
|
|
return &idTable[i]; /* found it */
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
ID_Robot_e ID_WhichDevice(void) {
|
|
if (currID==ID_ROBOT_NONE) {
|
|
/* not checked ID, try to find matching robot */
|
|
currID = IdentifyDevice();
|
|
}
|
|
return currID;
|
|
}
|
|
|
|
static uint8_t PrintStatus(const McuShell_StdIOType *io) {
|
|
uint8_t buf[32];
|
|
const ID_RobotDevice *device;
|
|
|
|
McuShell_SendStatusStr((unsigned char*)"id", (unsigned char*)"Identify status\r\n", io->stdOut);
|
|
device = GetDeviceDesc(ID_WhichDevice());
|
|
if (device!=NULL) {
|
|
McuUtility_strcpy(buf, sizeof(buf), device->name);
|
|
McuUtility_strcat(buf, sizeof(buf), (uint8_t*)"\r\n");
|
|
McuShell_SendStatusStr((unsigned char*)" device", buf, io->stdOut);
|
|
} else {
|
|
McuShell_SendStatusStr((unsigned char*)" device", (uint8_t*)"UNKNOWN\r\n", io->stdOut);
|
|
}
|
|
return ERR_OK;
|
|
}
|
|
|
|
uint8_t ID_ParseCommand(const unsigned char *cmd, bool *handled, const McuShell_StdIOType *io) {
|
|
if (McuUtility_strcmp((char*)cmd, McuShell_CMD_HELP)==0 || McuUtility_strcmp((char*)cmd, "id help")==0) {
|
|
McuShell_SendHelpStr((unsigned char*)"id", (const unsigned char*)"Group of id commands\r\n", io->stdOut);
|
|
McuShell_SendHelpStr((unsigned char*)" help|status", (const unsigned char*)"Print help or status information\r\n", io->stdOut);
|
|
*handled = TRUE;
|
|
return ERR_OK;
|
|
} else if ((McuUtility_strcmp((char*)cmd, McuShell_CMD_STATUS)==0) || (McuUtility_strcmp((char*)cmd, "id status")==0)) {
|
|
*handled = TRUE;
|
|
return PrintStatus(io);
|
|
}
|
|
return ERR_OK;
|
|
}
|
|
|
|
void ID_Deinit(void) {
|
|
currID = ID_ROBOT_NONE;
|
|
}
|
|
|
|
void ID_Init(void) {
|
|
currID = ID_ROBOT_NONE;
|
|
}
|
|
#endif /* PL_CONFIG_USE_IDENTIFY */
|
|
|