Yuki Suga / RTnoV3

RTno is communicating library and framework which allows you to make your embedded device capable of communicating with RT-middleware world. RT-middleware is a platform software to realize Robotic system. In RTM, robots are developed by constructing robotics technologies\' elements (components) named RT-component. Therefore, the RTno helps you to create your own RT-component with your mbed and arduino. To know how to use your RTno device, visit here: http://ysuga.net/robot_e/rtm_e/rtc_e/1065?lang=en To know about RT-middleware and RT-component, visit http://www.openrtm.org

Dependencies:   EthernetNetIf

Dependents:   RTnoV3_LED RTnoV3_Template RTnoV3_ADC RTnoV3_Timer ... more

RTno.cpp@0:9fac71a0bff3, 2012-02-09 (annotated)

Committer:
ysuga
Date:
Thu Feb 09 02:33:10 2012 +0000
Revision:
0:9fac71a0bff3
RTno Version 3: RTno is a software library and tool to connect embedded devices like arduino and mbed to RT-middleware world.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ysuga 0:9fac71a0bff3 1 /*******************************************
ysuga 0:9fac71a0bff3 2 * RTno.cpp
ysuga 0:9fac71a0bff3 3 * @author Yuki Suga
ysuga 0:9fac71a0bff3 4 * @copyright Yuki Suga (ysuga.net) Nov, 10th, 2010.
ysuga 0:9fac71a0bff3 5 * @license LGPLv3
ysuga 0:9fac71a0bff3 6 *****************************************/
ysuga 0:9fac71a0bff3 7 #define RTNO_SUBMODULE_DEFINE
ysuga 0:9fac71a0bff3 8 #include <stdint.h>
ysuga 0:9fac71a0bff3 9 #include "mbed.h"
ysuga 0:9fac71a0bff3 10
ysuga 0:9fac71a0bff3 11 #include "RTno.h"
ysuga 0:9fac71a0bff3 12 #include "Packet.h"
ysuga 0:9fac71a0bff3 13
ysuga 0:9fac71a0bff3 14 #include "Transport.h"
ysuga 0:9fac71a0bff3 15 //#include "UART.h"
ysuga 0:9fac71a0bff3 16 #include "RTnoProfile.h"
ysuga 0:9fac71a0bff3 17
ysuga 0:9fac71a0bff3 18 using namespace RTno;
ysuga 0:9fac71a0bff3 19
ysuga 0:9fac71a0bff3 20 // global variables
ysuga 0:9fac71a0bff3 21 // module private variables.
ysuga 0:9fac71a0bff3 22 #define PRIVATE static
ysuga 0:9fac71a0bff3 23
ysuga 0:9fac71a0bff3 24 // static value declaration.
ysuga 0:9fac71a0bff3 25 void(*SerialDevice_putc)(const char c);
ysuga 0:9fac71a0bff3 26 uint8_t(*SerialDevice_available)();
ysuga 0:9fac71a0bff3 27 char(*SerialDevice_getc)();
ysuga 0:9fac71a0bff3 28
ysuga 0:9fac71a0bff3 29 PRIVATE int8_t m_pPacketBuffer[PACKET_BUFFER_SIZE];
ysuga 0:9fac71a0bff3 30
ysuga 0:9fac71a0bff3 31 /*
ysuga 0:9fac71a0bff3 32 * Send Profile Data
ysuga 0:9fac71a0bff3 33 */
ysuga 0:9fac71a0bff3 34 PRIVATE void _SendProfile();
ysuga 0:9fac71a0bff3 35
ysuga 0:9fac71a0bff3 36 /**
ysuga 0:9fac71a0bff3 37 * Packet Handler in Error State.
ysuga 0:9fac71a0bff3 38 */
ysuga 0:9fac71a0bff3 39 PRIVATE void _PacketHandlerOnError();
ysuga 0:9fac71a0bff3 40
ysuga 0:9fac71a0bff3 41 /**
ysuga 0:9fac71a0bff3 42 * Packet Handler in Inactive State.
ysuga 0:9fac71a0bff3 43 */
ysuga 0:9fac71a0bff3 44 PRIVATE void _PacketHandlerOnInactive();
ysuga 0:9fac71a0bff3 45
ysuga 0:9fac71a0bff3 46 /**
ysuga 0:9fac71a0bff3 47 * Packet Handler in Active State.
ysuga 0:9fac71a0bff3 48 */
ysuga 0:9fac71a0bff3 49 PRIVATE void _PacketHandlerOnActive();
ysuga 0:9fac71a0bff3 50
ysuga 0:9fac71a0bff3 51 void EC_setup(exec_cxt_str& exec_cxt);
ysuga 0:9fac71a0bff3 52 void Connection_setup(config_str& conf);
ysuga 0:9fac71a0bff3 53
ysuga 0:9fac71a0bff3 54 /**
ysuga 0:9fac71a0bff3 55 * Arduino Setup Routine.
ysuga 0:9fac71a0bff3 56 * This function is called when arduino device is turned on.
ysuga 0:9fac71a0bff3 57 */
ysuga 0:9fac71a0bff3 58 void setup() {
ysuga 0:9fac71a0bff3 59 RTnoProfile_init();
ysuga 0:9fac71a0bff3 60 // This function must be called first.
ysuga 0:9fac71a0bff3 61 exec_cxt_str* exec_cxt = (exec_cxt_str*)malloc(sizeof(exec_cxt_str));
ysuga 0:9fac71a0bff3 62 config_str* conf = (config_str*)malloc(sizeof(config_str));
ysuga 0:9fac71a0bff3 63 rtcconf(*conf, *exec_cxt);
ysuga 0:9fac71a0bff3 64 if(onInitialize() == RTC_OK) {
ysuga 0:9fac71a0bff3 65 EC_setup(*exec_cxt);
ysuga 0:9fac71a0bff3 66 Connection_setup(*conf);
ysuga 0:9fac71a0bff3 67 free(exec_cxt);
ysuga 0:9fac71a0bff3 68 free(conf);
ysuga 0:9fac71a0bff3 69 Transport_init();
ysuga 0:9fac71a0bff3 70 EC_start();
ysuga 0:9fac71a0bff3 71 }
ysuga 0:9fac71a0bff3 72 }
ysuga 0:9fac71a0bff3 73
ysuga 0:9fac71a0bff3 74
ysuga 0:9fac71a0bff3 75 /**
ysuga 0:9fac71a0bff3 76 * Arduino Loop routine.
ysuga 0:9fac71a0bff3 77 * This function is repeadedly called when arduino is turned on.
ysuga 0:9fac71a0bff3 78 */
ysuga 0:9fac71a0bff3 79 void loop() {
ysuga 0:9fac71a0bff3 80 int8_t ret;
ysuga 0:9fac71a0bff3 81 ret = Transport_ReceivePacket(m_pPacketBuffer);
ysuga 0:9fac71a0bff3 82 if(ret < 0) { // Timeout Error or Checksum Error
ysuga 0:9fac71a0bff3 83 Transport_SendPacket(PACKET_ERROR, 1, &ret);
ysuga 0:9fac71a0bff3 84 } else if (ret == 0) {
ysuga 0:9fac71a0bff3 85 } else if (ret > 0) { // Packet is successfully received
ysuga 0:9fac71a0bff3 86 if (m_pPacketBuffer[INTERFACE] == GET_PROFILE) {
ysuga 0:9fac71a0bff3 87 _SendProfile();
ysuga 0:9fac71a0bff3 88 } else if(m_pPacketBuffer[INTERFACE] == GET_STATUS) {
ysuga 0:9fac71a0bff3 89 int8_t state = EC_get_component_state();
ysuga 0:9fac71a0bff3 90 Transport_SendPacket(GET_STATUS, 1, &state);
ysuga 0:9fac71a0bff3 91 } else if(m_pPacketBuffer[INTERFACE] == GET_CONTEXT) {
ysuga 0:9fac71a0bff3 92 int8_t type = EC_get_type();
ysuga 0:9fac71a0bff3 93 Transport_SendPacket(GET_CONTEXT, 1, &type);
ysuga 0:9fac71a0bff3 94 } else {
ysuga 0:9fac71a0bff3 95 switch(EC_get_component_state()) {
ysuga 0:9fac71a0bff3 96 case RTC_STATE_ERROR:
ysuga 0:9fac71a0bff3 97 _PacketHandlerOnError();
ysuga 0:9fac71a0bff3 98 break;
ysuga 0:9fac71a0bff3 99 case RTC_STATE_INACTIVE:
ysuga 0:9fac71a0bff3 100 _PacketHandlerOnInactive();
ysuga 0:9fac71a0bff3 101 break;
ysuga 0:9fac71a0bff3 102 case RTC_STATE_ACTIVE:
ysuga 0:9fac71a0bff3 103 _PacketHandlerOnActive();
ysuga 0:9fac71a0bff3 104 break;
ysuga 0:9fac71a0bff3 105 case RTC_STATE_NONE:
ysuga 0:9fac71a0bff3 106 ret = RTNO_NONE;
ysuga 0:9fac71a0bff3 107 Transport_SendPacket(m_pPacketBuffer[INTERFACE], 1, &ret);
ysuga 0:9fac71a0bff3 108 break;
ysuga 0:9fac71a0bff3 109 default: // if m_Condition is unknown...
ysuga 0:9fac71a0bff3 110
ysuga 0:9fac71a0bff3 111 break;
ysuga 0:9fac71a0bff3 112 }
ysuga 0:9fac71a0bff3 113 }
ysuga 0:9fac71a0bff3 114 }
ysuga 0:9fac71a0bff3 115
ysuga 0:9fac71a0bff3 116
ysuga 0:9fac71a0bff3 117 int numOutPort = RTnoProfile_getNumOutPort();
ysuga 0:9fac71a0bff3 118 for(int i = 0;i < numOutPort;i++) {
ysuga 0:9fac71a0bff3 119 EC_suspend();
ysuga 0:9fac71a0bff3 120 PortBase* pOutPort = RTnoProfile_getOutPortByIndex(i);
ysuga 0:9fac71a0bff3 121 if(pOutPort->pPortBuffer->hasNext(pOutPort->pPortBuffer)) {
ysuga 0:9fac71a0bff3 122 char* name = pOutPort->pName;
ysuga 0:9fac71a0bff3 123 unsigned char nameLen = strlen(name);
ysuga 0:9fac71a0bff3 124 unsigned char dataLen = pOutPort->pPortBuffer->getNextDataSize(pOutPort->pPortBuffer);
ysuga 0:9fac71a0bff3 125
ysuga 0:9fac71a0bff3 126 m_pPacketBuffer[0] = nameLen;
ysuga 0:9fac71a0bff3 127 m_pPacketBuffer[1] = dataLen;
ysuga 0:9fac71a0bff3 128 memcpy(m_pPacketBuffer + 2, name, nameLen);
ysuga 0:9fac71a0bff3 129 pOutPort->pPortBuffer->pop(pOutPort->pPortBuffer, m_pPacketBuffer + 2 + nameLen, dataLen);
ysuga 0:9fac71a0bff3 130 Transport_SendPacket(RECEIVE_DATA, 2 + nameLen + dataLen, m_pPacketBuffer);
ysuga 0:9fac71a0bff3 131 }
ysuga 0:9fac71a0bff3 132 EC_resume();
ysuga 0:9fac71a0bff3 133 }
ysuga 0:9fac71a0bff3 134
ysuga 0:9fac71a0bff3 135 }
ysuga 0:9fac71a0bff3 136
ysuga 0:9fac71a0bff3 137 /**
ysuga 0:9fac71a0bff3 138 * add InPort data to Profile.
ysuga 0:9fac71a0bff3 139 */
ysuga 0:9fac71a0bff3 140 void addInPort(InPortBase& Port)
ysuga 0:9fac71a0bff3 141 {
ysuga 0:9fac71a0bff3 142 RTnoProfile_addInPort(&Port);
ysuga 0:9fac71a0bff3 143 }
ysuga 0:9fac71a0bff3 144
ysuga 0:9fac71a0bff3 145 /**
ysuga 0:9fac71a0bff3 146 * add OutPort data to Profile
ysuga 0:9fac71a0bff3 147 */
ysuga 0:9fac71a0bff3 148 void addOutPort(OutPortBase& Port)
ysuga 0:9fac71a0bff3 149 {
ysuga 0:9fac71a0bff3 150 RTnoProfile_addOutPort(&Port);
ysuga 0:9fac71a0bff3 151 }
ysuga 0:9fac71a0bff3 152
ysuga 0:9fac71a0bff3 153
ysuga 0:9fac71a0bff3 154 /**
ysuga 0:9fac71a0bff3 155 * Private Function Definitions
ysuga 0:9fac71a0bff3 156 *
ysuga 0:9fac71a0bff3 157 */
ysuga 0:9fac71a0bff3 158
ysuga 0:9fac71a0bff3 159
ysuga 0:9fac71a0bff3 160 /**
ysuga 0:9fac71a0bff3 161 * Send Profile Data
ysuga 0:9fac71a0bff3 162 */
ysuga 0:9fac71a0bff3 163 PRIVATE void _SendProfile() {
ysuga 0:9fac71a0bff3 164 int8_t ret = RTNO_OK;
ysuga 0:9fac71a0bff3 165 for(uint8_t i = 0;i < RTnoProfile_getNumInPort();i++) {
ysuga 0:9fac71a0bff3 166 PortBase* inPort = RTnoProfile_getInPortByIndex(i);
ysuga 0:9fac71a0bff3 167 uint8_t nameLen = strlen(inPort->pName);
ysuga 0:9fac71a0bff3 168 m_pPacketBuffer[0] = inPort->typeCode;
ysuga 0:9fac71a0bff3 169 memcpy(&(m_pPacketBuffer[1]), inPort->pName, nameLen);
ysuga 0:9fac71a0bff3 170 Transport_SendPacket(ADD_INPORT, 1+nameLen, m_pPacketBuffer);
ysuga 0:9fac71a0bff3 171 }
ysuga 0:9fac71a0bff3 172
ysuga 0:9fac71a0bff3 173 for(uint8_t i = 0;i < RTnoProfile_getNumOutPort();i++) {
ysuga 0:9fac71a0bff3 174 PortBase* outPort = RTnoProfile_getOutPortByIndex(i);
ysuga 0:9fac71a0bff3 175 uint8_t nameLen = strlen(outPort->pName);
ysuga 0:9fac71a0bff3 176 m_pPacketBuffer[0] = outPort->typeCode;
ysuga 0:9fac71a0bff3 177 memcpy(&(m_pPacketBuffer[1]), outPort->pName, nameLen);
ysuga 0:9fac71a0bff3 178 Transport_SendPacket(ADD_OUTPORT, 1+nameLen, m_pPacketBuffer);
ysuga 0:9fac71a0bff3 179 }
ysuga 0:9fac71a0bff3 180
ysuga 0:9fac71a0bff3 181 Transport_SendPacket(GET_PROFILE, 1, &ret);
ysuga 0:9fac71a0bff3 182 }
ysuga 0:9fac71a0bff3 183
ysuga 0:9fac71a0bff3 184
ysuga 0:9fac71a0bff3 185
ysuga 0:9fac71a0bff3 186 /**
ysuga 0:9fac71a0bff3 187 * Packet Handler in Error State
ysuga 0:9fac71a0bff3 188 */
ysuga 0:9fac71a0bff3 189 PRIVATE void _PacketHandlerOnError() {
ysuga 0:9fac71a0bff3 190 char intface;
ysuga 0:9fac71a0bff3 191 int8_t ret = RTNO_OK;
ysuga 0:9fac71a0bff3 192
ysuga 0:9fac71a0bff3 193 int8_t retval = EC_error();
ysuga 0:9fac71a0bff3 194 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:9fac71a0bff3 195 Transport_SendPacket(intface, 1, &ret);
ysuga 0:9fac71a0bff3 196 }
ysuga 0:9fac71a0bff3 197
ysuga 0:9fac71a0bff3 198
ysuga 0:9fac71a0bff3 199 /**
ysuga 0:9fac71a0bff3 200 * Packet Handler in Inactive State
ysuga 0:9fac71a0bff3 201 */
ysuga 0:9fac71a0bff3 202 PRIVATE void _PacketHandlerOnInactive() {
ysuga 0:9fac71a0bff3 203
ysuga 0:9fac71a0bff3 204 int8_t ret = RTNO_OK;
ysuga 0:9fac71a0bff3 205 int8_t retval = EC_activate_component();
ysuga 0:9fac71a0bff3 206 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:9fac71a0bff3 207 Transport_SendPacket(ACTIVATE, 1, &ret);
ysuga 0:9fac71a0bff3 208 }
ysuga 0:9fac71a0bff3 209
ysuga 0:9fac71a0bff3 210 /**
ysuga 0:9fac71a0bff3 211 * Packet Handler in Active State.
ysuga 0:9fac71a0bff3 212 */
ysuga 0:9fac71a0bff3 213 PRIVATE void _PacketHandlerOnActive() {
ysuga 0:9fac71a0bff3 214 int8_t ret = RTNO_OK;
ysuga 0:9fac71a0bff3 215 //char intface;
ysuga 0:9fac71a0bff3 216 int8_t retval;
ysuga 0:9fac71a0bff3 217 switch(m_pPacketBuffer[INTERFACE]) {
ysuga 0:9fac71a0bff3 218 case DEACTIVATE:
ysuga 0:9fac71a0bff3 219 retval = EC_deactivate_component();
ysuga 0:9fac71a0bff3 220 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:9fac71a0bff3 221 Transport_SendPacket(DEACTIVATE, 1, &ret);
ysuga 0:9fac71a0bff3 222 break;
ysuga 0:9fac71a0bff3 223 case EXECUTE:
ysuga 0:9fac71a0bff3 224 retval = EC_execute();
ysuga 0:9fac71a0bff3 225 if(retval < 0) ret = RTNO_ERROR;
ysuga 0:9fac71a0bff3 226 Transport_SendPacket(EXECUTE, 1, &ret);
ysuga 0:9fac71a0bff3 227 break;
ysuga 0:9fac71a0bff3 228 case SEND_DATA: {
ysuga 0:9fac71a0bff3 229 PortBase* pInPort = RTnoProfile_getInPort((const char*)&(m_pPacketBuffer[DATA_START_ADDR+2]), m_pPacketBuffer[DATA_START_ADDR]);
ysuga 0:9fac71a0bff3 230 if(pInPort == NULL) {
ysuga 0:9fac71a0bff3 231
ysuga 0:9fac71a0bff3 232 } else {
ysuga 0:9fac71a0bff3 233 PortBuffer* pBuffer = pInPort->pPortBuffer;
ysuga 0:9fac71a0bff3 234 EC_suspend();
ysuga 0:9fac71a0bff3 235 pBuffer->push(pBuffer,&(m_pPacketBuffer[DATA_START_ADDR+2+m_pPacketBuffer[DATA_START_ADDR]]), m_pPacketBuffer[DATA_START_ADDR+1]);
ysuga 0:9fac71a0bff3 236 EC_resume();
ysuga 0:9fac71a0bff3 237 Transport_SendPacket(SEND_DATA, 1, &ret);
ysuga 0:9fac71a0bff3 238 }
ysuga 0:9fac71a0bff3 239 }
ysuga 0:9fac71a0bff3 240 break;
ysuga 0:9fac71a0bff3 241 default:
ysuga 0:9fac71a0bff3 242 break;
ysuga 0:9fac71a0bff3 243 }
ysuga 0:9fac71a0bff3 244 }
ysuga 0:9fac71a0bff3 245