void usb_lp_can_rx0_isr(void)
{
u32 id, fmi;
bool ext, rtr;
u8 length, data[8];
can_receive(CAN1, 0, false, &id, &ext, &rtr, &fmi, &length, data);
if (data[0] & 1)
gpio_clear(GPIOA, GPIO8);
else
gpio_set(GPIOA, GPIO8);
if (data[0] & 2)
gpio_clear(GPIOB, GPIO4);
else
gpio_set(GPIOB, GPIO4);
if (data[0] & 4)
gpio_clear(GPIOC, GPIO15);
else
gpio_set(GPIOC, GPIO15);
if (data[0] & 8)
gpio_clear(GPIOC, GPIO2);
else
gpio_set(GPIOC, GPIO2);
can_fifo_release(CAN1, 0);
}
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;
}
void usb_lp_can_rx0_isr(void) {
uint32_t id, fmi;
bool ext, rtr;
uint8_t dlc, data[8];
can_receive(CAN1, 0, false, &id, &ext, &rtr, &fmi, &dlc, data);
can_fifo_release(CAN1, 0);
/* check for extended id, dlc = 5 and id 0x0000xxxx */
if ((ext) && (dlc == 5) && !(id & 0xffff0000)) {
/* M*rklin Start/Stop CMD
00004711 [5] 00 00 00 00 00 -> Stop
00004711 [5] 00 00 00 00 01 -> Start
*/
if (data[4] && 0x01)
if (!status) {
/* send stop immediatly if Stop Button pressed */
can_transmit(CAN1, 0x0000fffe, true, false, 5, data);
gpio_set(GPIOC, GPIO13); /* clear green LED */
gpio_set(GPIOB, GPIO4); /* clear On LED */
gpio_clear(GPIOB, GPIO5); /* light Off LED */
} else {
gpio_clear(GPIOC, GPIO13); /* light green LED */
gpio_clear(GPIOB, GPIO4); /* light On LED */
gpio_set(GPIOB, GPIO5); /* clear Off LED */
} else {
gpio_set(GPIOC, GPIO13); /* clear green LED */
gpio_set(GPIOB, GPIO4); /* clear On LED */
gpio_clear(GPIOB, GPIO5); /* light Off LED */
}
}
}
bool can_interface_read_message(uint32_t *id, uint8_t *message, uint8_t *length, uint32_t retries)
{
uint32_t fid;
uint8_t len;
bool ext, rtr;
while(retries-- != 0 && (CAN_RF0R(CAN1) & CAN_RF0R_FMP0_MASK) == 0);
if ((CAN_RF0R(CAN1) & CAN_RF0R_FMP0_MASK) == 0) {
return false;
}
can_receive(
CAN1, // canport
0, // fifo
true, // release
id, // can id
&ext, // extended id
&rtr, // transmission request
&fid, // filter id
&len, // length
message
);
*length = len;
return true;
}
void ObjectProxy::set_value(const Value &val) {
assert(root_proxy_);
// std::cout << url() << ": set_value(" << val << ")\n";
if (can_receive(val)) {
time_t now = time_ref_.elapsed();
// only send if value type is correct
if (latency_ < 0) {
if (wait_until_ >= 0) {
// waiting for first call reply: wait
// std::cout << url() << ": waiting for first call reply: wait\n";
} else {
// first call
root_proxy_->send_to_remote(url().c_str(), val);
wait_until_ = now;
}
} else if (now < wait_until_) {
// do not send
// std::cout << url() << ": to send\n";
to_send_ = val;
} else {
root_proxy_->send_to_remote(url().c_str(), val);
wait_until_ = now + (latency_ * 1.5);
}
}
}
int aseba_can_receive(uint16_t *msg)
{
if (CAN_RF0R(CAN1) & CAN_RF0R_FMP0_MASK) {
uint32_t id, fmi;
bool ext, rtr;
uint8_t length;
uint8_t data[8];
can_receive(CAN1, 0, true, &id, &ext, &rtr, &fmi, &length, data);
// uart_puts("can message received\n");
if (length % 2 != 0) {
return -1;
}
// if ((id & 0x300) != ASEBA_TYPE_SMALL_PACKET) {
// return -1;
// }
int aseba_length = length / 2;
int i;
for (i = 0; i < aseba_length; i++) {
msg[i] = data[i*2] + (data[i*2 + 1]<<8); // aseba data is little endian
}
return aseba_length;
} else {
return -1;
}
}
void usb_lp_can_rx0_isr(void)
{
uint32_t id, fmi;
bool ext, rtr;
uint8_t length, data[8];
can_receive(CAN1, 0, false, &id, &ext, &rtr, &fmi, &length, data);
if (data[0] & 1)
gpio_clear(GPIOA, GPIO6);
else
gpio_set(GPIOA, GPIO6);
if (data[0] & 2)
gpio_clear(GPIOA, GPIO7);
else
gpio_set(GPIOA, GPIO7);
if (data[0] & 4)
gpio_clear(GPIOB, GPIO0);
else
gpio_set(GPIOB, GPIO0);
if (data[0] & 8)
gpio_clear(GPIOB, GPIO1);
else
gpio_set(GPIOB, GPIO1);
can_fifo_release(CAN1, 0);
}
void main()
{
BYTE cpt_RB4;
BYTE cpt_RB5;
Command cmd;
BYTE param;
int run = 1;
init();
cpt_RB4 = 0;
cpt_RB5 = 0;
avancement = 0;
while(run)
{
if (1 == can_receive(&cmd, ¶m))
{
switch (cmd)
{
case Avancer:
avancer(param);
break;
case Reculer:
reculer(param);
break;
case Degree:
degree();
break;
case Reset:
run = 0;
break;
default:
break;
}
}
if(bouton_RB4_pressed())
{
cpt_RB4++;
can_send(BoutonRB4, cpt_RB4);
}
if(bouton_RB5_pressed())
{
cpt_RB5++;
can_send(BoutonRB5, cpt_RB5);
}
}
}
bool Slot::add_connection(Slot *slot) {
if (type_id() == NO_TYPE_TAG_ID || slot->type_id() == NO_TYPE_TAG_ID) return false; // one of them is a NoIO
assert(type().size() > 0);
assert(slot->type().size() > 0);
if ((kind_of(Inlet) && can_receive(slot->type()[0])) ||
(slot->kind_of(Inlet) && slot->can_receive(type()[0]))) {
// same type signature or inlet receiving any type
// OrderedList makes sure the link is not created again if it already exists.
connections_.push(slot);
return true;
} else {
return false;
}
}
//
// The polling process that gets characters
//
void getChars(void *user)
{
while(1) {
double d = DEFAULT_RX_DELAY;
bhmWaitDelay(d);
if (can_receive()) {
Uns8 c;
if (bhmSerRead(channel, &c, 1)) {
bhmTriggerEvent(charReceived);
receiveByte(c);
}
}
}
}
void ClientNetSocket::add_recv_tokens(uint32_t num_tokens)
{
if ((_status == SOCKET_STATUS_CLOSED) || _close_pending) {
THROW("%s: ack attempt for disconnected socket connection_id=%d", __FUNCTION__,
_id);
}
_num_recv_tokens += num_tokens;
// recv might have not been called because tokens weren't available
if (_num_recv_tokens && (_num_recv_tokens == num_tokens)) {
if (can_receive()) {
receive();
}
}
}
void ClientNetSocket::on_event()
{
if (can_send()) {
send();
}
if (!during_send()) {
if (_close_pending) {
close_and_tell();
} else if (_fin_pending) {
apply_guest_fin();
}
}
if (can_receive()) {
receive();
}
}
void usb_lp_can_rx0_isr(void)
{
uint32_t id, fmi;
bool ext, rtr;
uint8_t length, data[8];
can_receive(CAN1,
0, /* FIFO: 0 */
false, /* Release */
&id,
&ext,
&rtr,
&fmi,
&length,
data);
_can_run_rx_callback(id, data, length);
can_fifo_release(CAN1, 0);
}
void can1_rx0_isr(void){
uint32_t id;
uint8_t fmi;
bool ext, rtr;
uint8_t length, data[8];
uint16_t timestamp;
can_receive(CAN1,
0, /* FIFO: 0 */
false, /* Release */
&id,
&ext,
&rtr,
&fmi,
&length,
data,
×tamp);
_can_run_rx_callback(id, data, length);
can_fifo_release(CAN1, 0);
}
void usb_lp_can_rx0_isr(void)
{
static int delay = 100;
can_receive(CAN1, 0, false,
&can_rx_msg.id,
&can_rx_msg.ide,
&can_rx_msg.rtr,
&can_rx_msg.fmi,
&can_rx_msg.dlc,
can_rx_msg.data);
can_fifo_release(CAN1, 0);
if (can_rx_msg.data[0] & 1) {
ON(LED_ORANGE);
} else {
OFF(LED_ORANGE);
}
if (can_rx_msg.data[0] & 2) {
ON(LED_GREEN);
} else {
OFF(LED_GREEN);
}
if(delay == 0) {
delay = 100;
TOGGLE(LED_RED);
}else{
delay--;
}
TOGGLE(LED_BLUE);
//_can_run_rx_callback(can_rx_msg.id, can_rx_msg.data, can_rx_msg.dlc);
}
void vControl ( void *pvParameters )
{
portTickType xLastWakeTime;
signed char valx, valy;
unsigned char ls, rs, lb, rb;
can_frame_t out_frame;
can_frame_t in_frame;
out_frame.id = 1;
out_frame.dlc = 6;
xLastWakeTime = xTaskGetTickCount ();
/* Button init */
buttons_init ();
/* FSM init */
fsm_init ();
/* CAN init */
can_init ();
/* ADC init */
adc_init ( ctrlNUM_ADC_VALUES );
/* Touch init */
touch_init ( 30, 30, 30, 30 );
while (1)
{
vTaskDelayUntil ( &xLastWakeTime, ctrlTASK_FREQUENCY );
if ( adc_conversion_complete () == pdTRUE )
{
adc_disable ();
adc_get_value ( &valx, 0 );
adc_get_value ( &valy, 0 );
touch_measure ( &ls, &rs, &lb, &rb );
out_frame.data[0] = valx;
out_frame.data[1] = valy;
out_frame.data[2] = ls;
out_frame.data[3] = rs;
out_frame.data[4] = lb;
out_frame.data[5] = rb;
if (fsm_get_state() == ST_PLAY)
{
can_transmit (&out_frame);
}
can_receive (&in_frame, 0);
if (in_frame.data[0] == GAME_SENSOR_TRIGGERED)
{
// TODO: set triggered state
fsm_event_t *event = pvPortMalloc (sizeof (fsm_event_t));
event->type = EV_STOP;
event->ptr = NULL;
fsm_event_put (event, portMAX_DELAY);
in_frame.data[0] = 0;
}
else if (in_frame.data[0] == GAME_SENSOR_CLEARED)
{
// TODO: set cleared state
in_frame.data[0] = 0;
}
adc_enable ();
adc_conversion_start ();
}
fsm_update ();
}
}
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!"
}
int main(void) {
set_canit_callback(CANIT_RX_COMPLETED, rx_complete);
set_canit_callback(CANIT_TX_COMPLETED, tx_complete);
set_canit_callback(CANIT_DEFAULT, can_default);
usart1_init();
CAN_INIT_ALL(); //Can setup
sei(); //Enable interrupt
usart1_printf("\n\n\nSTARTING\n");
// Set MOB 8 to listen for messeges with id 4 and length 7
can_msg_t rx_msg = {
.mob = 8,
.id = 4,
.dlc = 7,
.mode = MOB_RECIEVE
};
can_setup(&rx_msg);
usart1_printf("Listning for id %d on mob %d with a msg length %d\n",
rx_msg.id,
rx_msg.mob,
rx_msg.dlc
);
while(1){
// Main work loop
_delay_ms(250);
// send a message with id 4 on MOB 10
can_msg_t tx_msg = {
.mob = 10,
.id = 4,
.data = {'H', 'E', 'L', 'L', 'O', '!', '!'},
.dlc = 7,
.mode = MOB_TRANSMIT
};
can_send(&tx_msg);
usart1_printf("CAN Tx\t id %d, mob %d, :: ", tx_msg.id, tx_msg.mob);
// As tx_msg.data is a byte array we cant treat it as a string
usart1_putn(sizeof(tx_msg.data)/sizeof(tx_msg.data[0]), tx_msg.data);
usart1_putc('\n');
}
return 0;
}
// Callback to be run when rx comletes on the CAN
static void rx_complete(uint8_t mob) {
can_msg_t msg = {
.mob = mob // mob is the MOB that fired the interrupt
};
can_receive(&msg); // Fetch the message and fill out the msg struct
// Print out the received data. Please dont print inside can callbacks
// in real code as these are run inside the can ISR
usart1_printf("CAN Rx\t id: %d on mob %d :: ", msg.id, msg.mob);
usart1_putn(msg.dlc, msg.data); usart1_putc('\n');
}
static void tx_complete(uint8_t mob) {
// We clear the mob so it can be used again
MOB_ABORT(); // Freed the MOB
MOB_CLEAR_INT_STATUS(); // and reset MOB status
CAN_DISABLE_MOB_INTERRUPT(mob); // Unset interrupt
}
