// this is called by the eeprom init task bool eeprom_init_done(semaphore_t *worker_task) { switch(init_state) { case config_can : if(can_config(memid_nodeid, worker_task)) init_state = config_serial; break; case config_serial : // TODO: read other config values that are set.... uart_config.rate = get_parameter_type_1(memid_baudrate)->value.SHORT; init_deque(&uart_config.rx_queue, RX_BUFFER_LENGTH, rx_queue, NUM_RX_BUFFERS); init_uart(&uart_config, rx_stack, numelements(rx_stack), tx_stack, numelements(tx_stack)); signal(worker_task); init_state = config_complete; break; default : return true; // init is done } return false; // more work }
LONG cop_request(LONG cob_id, SHORT *length, BYTE *data) { WORD timeout[9] = {2,2,2,2,2,2,5,10,20}; BYTE dlc = (BYTE)*length; // 1. Configure receive message object with remote request if((cop_error = can_config(CANBUF_RX, cob_id, (WORD)CAN_REQUEST(dlc))) != CANERR_NOERROR) { can_delete(CANBUF_RX); return cop_error; } // 2. Start timer (increased time-out for RTR-frames) can_start_timer((WORD)(timeout[cop_baudrate] * CANRTR_FACTOR)); // 3. Wait until message is received do { // Return if a message is received, or an error occurred if((cop_error = can_receive(CANBUF_RX, length, data)) != COPERR_RX_EMPTY) { can_delete(CANBUF_RX); return cop_error; } } while(!can_is_timeout()); // 4. A time-out occurred! can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; }
LONG cop_transmit(LONG cob_id, SHORT length, BYTE *data) { // 1. Configure transmit message object if((cop_error = can_config(CANBUF_TX, cob_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) { can_delete(CANBUF_TX); return cop_error; } // 2. Transmit the message if((cop_error = can_transmit(CANBUF_TX, length, data)) != CANERR_NOERROR) { can_delete(CANBUF_TX); return cop_error; } // Return if the message is transmitted can_delete(CANBUF_TX); return cop_error; }
static LONG sdo_receive(BYTE node_id, WORD index, BYTE subindex, SHORT *length, BYTE *data, SHORT max) { short n; // data length code short rc; // return value short t = 0; // toggle bit // --- Initiate SDO Upload --- cop_buffer[0] = 0x40; // client command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = 0x00; // reserved: set to 00h cop_buffer[5] = 0x00; // -"- cop_buffer[6] = 0x00; // -"- cop_buffer[7] = 0x00; // -"- n = 8; // 8 bytes to transmit! // 1. Configure transmit message object for client SDO if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) { can_delete(CANBUF_TX); return cop_error; } // 2. Configure receive message object for server SDO if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 2. Transmit the client SDO message if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 3. Start timer for SDO time-out can_start_timer(cop_timeout); // 4. Wait until server message is received do { switch((rc = can_receive(CANBUF_RX, &n, cop_buffer))) { case CANERR_NOERROR: // confirmation: if(n != 8) { // 8 bytes received? cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_LENGTH; } if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB) (cop_buffer[2] != HIBYTE(index)) || // index (MSB) (cop_buffer[3] != (BYTE)(subindex))) { // subindex rc = COPERR_FORMAT; break; } if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received? LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB) LOHIBYTE(cop_error) = cop_buffer[5]; // -"- HILOBYTE(cop_error) = cop_buffer[6]; // -"- HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB) *length = 0; // no data received! // Return value is abort code! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } if((cop_buffer[0] & 0xE0) != 0x40) { // unknown command specifier? cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } if((cop_buffer[0] & 0x02) == 0x02) { // expedited transfer? if((cop_buffer[0] & 0x01) == 0x01) n = 4 - (short)((cop_buffer[0] & 0x0C) >> 2); else n = 4; memcpy(data, &cop_buffer[4], n < max? n : max); *length = n < max? n : max; // data received!!! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_NOERROR; } break; case CANERR_RX_EMPTY: // receiver empty: if(can_is_timeout()) { // time-out occurred? cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; } break; default: // other errors: cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = rc; } } while(rc != CANERR_NOERROR); // segmented transfer: // --- Upload SDO Segment --- for(*length = 0;;) { cop_buffer[0] = 0x60 | t; // client command specifier cop_buffer[1] = 0x00; // reserved: set to 00h cop_buffer[2] = 0x00; // -"- cop_buffer[3] = 0x00; // -"- cop_buffer[4] = 0x00; // -"- cop_buffer[5] = 0x00; // -"- cop_buffer[6] = 0x00; // -"- cop_buffer[7] = 0x00; // -"- n = 8; // 8 bytes to transmit! // 5. Transmit the client SDO message if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 6. Start timer for SDO time-out can_start_timer(cop_timeout); // 7. Wait until server message is received do { switch((rc = can_receive(CANBUF_RX, &n, cop_buffer))) { case CANERR_NOERROR: // confirmation: if(n != 8) { // 8 bytes received? cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_LENGTH; } if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received? LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB) LOHIBYTE(cop_error) = cop_buffer[5]; // -"- HILOBYTE(cop_error) = cop_buffer[6]; // -"- HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB) *length = 0; // no data received! // Return value is abort code! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } if((cop_buffer[0] & 0xE0) != 0x00) { // unknown command specifier? cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered? cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } if((cop_buffer[0] & 0x0E) != 0x00) // number of segment data bytes n = 7 - (int)((cop_buffer[0] & 0x0E) >> 1); else n = 7; if(max - *length > 0) // copy segment data if space memcpy(&data[*length], &cop_buffer[1], *length + n < max? n : max - *length); *length += n; if((cop_buffer[0] & 0x01) == 0x01) { // no more segments? if(*length > max) *length = max; // truncate to buffer size! if(*length < max) data[*length] = '\0'; // for zero-closed strings! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_NOERROR; } break; case CANERR_RX_EMPTY: // receiver empty: if(can_is_timeout()) { // time-out occurred? cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; } break; default: // other errors: cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) *length = 0; // no data received! // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = rc; } } while(rc != CANERR_NOERROR); t = t? 0x00 : 0x10; // alternate toggle bit! } }
static LONG sdo_segmented(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data) { short n, i; // data length code short rc; // return value short t = 0; // toggle bit // --- Initiate SDO Download --- cop_buffer[0] = (BYTE)0x21; // client command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOBYTE(length); // number of data bytes (LSB) cop_buffer[5] = HIBYTE(length); // -"- cop_buffer[6] = (BYTE)0x00; // -"- cop_buffer[7] = (BYTE)0x00; // number of data bytes (MSB) n = 8; // 8 bytes to transmit! // 1. Configure transmit message object for client SDO if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) { can_delete(CANBUF_TX); return cop_error; } // 2. Configure receive message object for server SDO if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 2. Transmit the client SDO message if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 3. Start timer for SDO time-out can_start_timer(cop_timeout); // 4. Wait until server message is received do { switch((rc = can_receive(CANBUF_RX, &n, cop_buffer))) { case CANERR_NOERROR: // confirmation: if(n != 8) { // 8 bytes received? cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_LENGTH; } if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB) (cop_buffer[2] != HIBYTE(index)) || // index (MSB) (cop_buffer[3] != (BYTE)(subindex))) { // subindex rc = COPERR_FORMAT; break; } if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received? LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB) LOHIBYTE(cop_error) = cop_buffer[5]; // -"- HILOBYTE(cop_error) = cop_buffer[6]; // -"- HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB) // Return value is abort code! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier? cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } case CANERR_RX_EMPTY: // receiver empty: if(can_is_timeout()) { // time-out occurred? cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; } break; default: // other errors: cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = rc; } } while(rc != CANERR_NOERROR); // --- Download SDO Segment --- for(i = 0;;) { if(length <= 7) // bytes that does not contain data n = 7 - length; else // no segment size indicated n = 0; cop_buffer[0] = (BYTE)(n << 1); // client command specifier memset(&cop_buffer[1], 0x00, 7);// clear data buffer memcpy(&cop_buffer[1], &data[i], 7 - n);// copy segment data length -= 7 - n; // remaining number of bytes i += 7 - n; // index to remainung bytes cop_buffer[0]|= length? 0x00 : 0x01;// last segment to transmit cop_buffer[0]|= t; // toggle bit n = 8; // 8 bytes to transmit! // 5. Transmit the client SDO message if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 6. Start timer for SDO time-out can_start_timer(cop_timeout); // 7. Wait until server message is received do { switch((rc = can_receive(CANBUF_RX, &n, cop_buffer))) { case CANERR_NOERROR: // confirmation: if(n != 8) { // 8 bytes received? cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_LENGTH; } if((cop_buffer[0] & 0xE0) == 0x80) { // SDO abort received? LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB) LOHIBYTE(cop_error) = cop_buffer[5]; // -"- HILOBYTE(cop_error) = cop_buffer[6]; // -"- HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB) // Return value is abort code! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } if((cop_buffer[0] & 0xE0) != 0x20) { // unknown command specifier? cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered? cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } if(length == 0) { // all data tranmitted? can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_NOERROR; } case CANERR_RX_EMPTY: // receiver empty: if(can_is_timeout()) { // time-out occurred? cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; } break; default: // other errors: cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = rc; } } while(rc != CANERR_NOERROR); t = t? 0x00 : 0x10; // alternate toggle bit! } }
static LONG sdo_expedited(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data) { short n; // data length code short rc; // return value // --- Expedited SDO Download --- cop_buffer[0] = (BYTE)0x23; // client command specifier cop_buffer[0] |= (BYTE)((4 - length) << 2); cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex memset(&cop_buffer[4],0x00,4); // clear data buffer memcpy(&cop_buffer[4],data,length); // copy data bytes n = 8; // 8 bytes to transmit! // 1. Configure transmit message object for client SDO if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) { can_delete(CANBUF_TX); return cop_error; } // 2. Configure receive message object for server SDO if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 2. Transmit the client SDO message if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) { can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } // 3. Start timer for SDO time-out can_start_timer(cop_timeout); // 4. Wait until server message is received do { switch((rc = can_receive(CANBUF_RX, &n, cop_buffer))) { case CANERR_NOERROR: // confirmation: if(n != 8) { // 8 bytes received? cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_LENGTH; } if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB) (cop_buffer[2] != HIBYTE(index)) || // index (MSB) (cop_buffer[3] != (BYTE)(subindex))) { // subindex rc = COPERR_FORMAT; break; } if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received? LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB) LOHIBYTE(cop_error) = cop_buffer[5]; // -"- HILOBYTE(cop_error) = cop_buffer[6]; // -"- HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB) // Return value is abort code! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error; } if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier? cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_FORMAT; } else { // success: data written! can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_NOERROR; } case CANERR_RX_EMPTY: // receiver empty: if(can_is_timeout()) { // time-out occurred? cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = COPERR_TIMEOUT; } break; default: // other errors: cop_error = SDOERR_GENERAL_ERROR; // abort: general error cop_buffer[0] = 0x80; // command specifier cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB) cop_buffer[2] = HIBYTE(index); // index (MSB) cop_buffer[3] = (BYTE)(subindex); // subindex cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB) cop_buffer[5] = LOHIBYTE(cop_error); // -"- cop_buffer[6] = HILOBYTE(cop_error); // -"- cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB) // Transmit SDO abort and return can_transmit(CANBUF_TX, 8, cop_buffer); can_delete(CANBUF_TX); can_delete(CANBUF_RX); return cop_error = rc; } } while(1); // "the torture never stops!" }