Changed the way the 16 bit variables are accessed in the incomming and...

Changed the way the 16 bit variables are accessed in the incomming and outgoing packets. STM32L0 family generated a system fault with the previous approach.

dynamixel_base/include/dynamixel_slave.h

@@ -6,15 +6,15 @@
 #include "dynamixel2.h"
 
 #ifndef MAX_DYN_SLAVE_TX_BUFFER_LEN
-  #define       MAX_DYN_SLAVE_TX_BUFFER_LEN      1024
+  #define       MAX_DYN_SLAVE_TX_BUFFER_LEN      256
 #endif
 
 #ifndef MAX_DYN_SLAVE_RX_BUFFER_LEN
-  #define       MAX_DYN_SLAVE_RX_BUFFER_LEN      1024
+  #define       MAX_DYN_SLAVE_RX_BUFFER_LEN      256
 #endif
 
 #ifndef MAX_DYN_SLAVE_REG_BUFFER_LEN
-  #define       MAX_DYN_SLAVE_REG_BUFFER_LEN     1024
+  #define       MAX_DYN_SLAVE_REG_BUFFER_LEN     256
 #endif
 
 typedef struct

dynamixel_base/src/dynamixel2.c

@@ -41,27 +41,29 @@ void dyn2_set_checksum(unsigned char *packet)
 {
   unsigned short int crc_accum=0x0000,i,j,length;
 
-  length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])+5;
+  length=dyn2_get_length(packet)+5;
   for(j=0;j<length;j++)
   {
     i=((unsigned short)(crc_accum>>8)^packet[j])&0xFF;
     crc_accum=(crc_accum<<8)^crc_table[i];
   }
-  
-  *((unsigned short int *)&packet[length])=crc_accum;
+ 
+  packet[length]=crc_accum%256; 
+  packet[length+1]=crc_accum/256; 
 }
 
 unsigned char dyn2_check_checksum(unsigned char *packet)
 {
   unsigned short int crc_accum=0x0000,i,j,length;
 
-  length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])+5;
+  length=dyn2_get_length(packet)+5;
   for(j=0;j<length;j++)
   {
     i=((unsigned short)(crc_accum>>8)^packet[j])&0xFF;
     crc_accum=(crc_accum<<8)^crc_table[i];
   }
-  if(*((unsigned short int *)&packet[length])==crc_accum)
+  crc_accum-=(packet[length]+packet[length+1]*256);
+  if(crc_accum==0)
     return 0x01;
   else
     return 0x00;
@@ -72,7 +74,7 @@ void dyn2_copy_packet(unsigned char *source, unsigned char *destination)
 {
   unsigned short int i,length;
 
-  length=*((unsigned short int *)&source[DYN2_LENGTH_OFF])+7;
+  length=dyn2_get_length(source)+7;
   for(i=0;i<length;i++)
     destination[i]=source[i];
 }
@@ -84,7 +86,7 @@ inline unsigned char dyn2_get_id(unsigned char *packet)
 
 inline unsigned short int dyn2_get_length(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_LENGTH_OFF]);
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256);
 }
 
 inline TDynInstruction dyn2_get_instruction(unsigned char *packet)
@@ -136,19 +138,21 @@ void dyn2_init_read_packet(unsigned char *packet,unsigned char id,unsigned short
   packet[DYN2_LENGTH_OFF]=0x07;
   packet[DYN2_LENGTH_OFF+1]=0x00;
   packet[DYN2_INST_OFF]=DYN_READ;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF])=address;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length;
+  packet[DYN2_DATA_OFF]=address%256;
+  packet[DYN2_DATA_OFF+1]=address/256;
+  packet[DYN2_DATA_OFF+2]=length%256;
+  packet[DYN2_DATA_OFF+3]=length/256;
   dyn2_set_checksum(packet);
 }
 
 inline unsigned short int dyn2_get_read_length(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_DATA_OFF+2]);
+  return (packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256);
 }
 
 inline unsigned short int dyn2_get_read_address(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
 }
 
 /* write instruction */
@@ -161,9 +165,11 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=id;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=5+length;
+  packet[DYN2_LENGTH_OFF]=(length+5)%256;
+  packet[DYN2_LENGTH_OFF+1]=(length+5)/256;
   packet[DYN2_INST_OFF]=DYN_WRITE;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF])=address;
+  packet[DYN2_DATA_OFF]=address%256;
+  packet[DYN2_DATA_OFF+1]=address/256;
   for(i=0;i<length;i++)
     packet[DYN2_DATA_OFF+i+2]=data[i];
   dyn2_set_checksum(packet);
@@ -171,12 +177,12 @@ void dyn2_init_write_packet(unsigned char *packet,unsigned char id,unsigned shor
 
 inline unsigned short int dyn2_get_write_address(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
 }
 
 inline unsigned short int dyn2_get_write_length(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5;
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
 }
 
 unsigned short int dyn2_get_write_data(unsigned char *packet,unsigned char *data)
@@ -200,9 +206,11 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=id;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=5+length;
+  packet[DYN2_LENGTH_OFF]=(length+5)%256;
+  packet[DYN2_LENGTH_OFF+1]=(length+5)/256;
   packet[DYN2_INST_OFF]=DYN_REG_WRITE;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF])=address;
+  packet[DYN2_DATA_OFF]=address%256;
+  packet[DYN2_DATA_OFF+1]=address/256;
   for(i=0;i<length;i++)
     packet[DYN2_DATA_OFF+i+2]=data[i];
   dyn2_set_checksum(packet);
@@ -210,12 +218,12 @@ void dyn2_init_reg_write_packet(unsigned char *packet,unsigned char id,unsigned
 
 inline unsigned short int dyn2_get_reg_write_address(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_DATA_OFF]);
+  return (packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256);
 }
 
 inline unsigned short int dyn2_get_reg_write_length(unsigned char *packet)
 {
-  return *((unsigned short int *)&packet[DYN2_LENGTH_OFF])-5;
+  return (packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-5;
 }
 
 unsigned short int dyn2_get_reg_write_data(unsigned char *packet,unsigned char *data)
@@ -262,17 +270,21 @@ void dyn2_init_reset_packet(unsigned char *packet,unsigned char id,unsigned char
 void dyn2_init_sync_write_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int address,unsigned short int length,TWriteData *data)
 {
   unsigned char i;
-  unsigned short int j;
+  unsigned short int j,len;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=0xFE;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=7+(length+1)*num_servos;
+  len=7+(length+1)*num_servos;
+  packet[DYN2_LENGTH_OFF]=len%256;
+  packet[DYN2_LENGTH_OFF+1]=len/256;
   packet[DYN2_INST_OFF]=DYN_SYNC_WRITE;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF])=address;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length;
+  packet[DYN2_DATA_OFF]=address%256;
+  packet[DYN2_DATA_OFF+1]=address/256;
+  packet[DYN2_DATA_OFF+2]=length%256;
+  packet[DYN2_DATA_OFF+3]=length/256;
   for(i=0;i<num_servos;i++)
   {
     packet[DYN2_DATA_OFF+4+(length+1)*i]=servo_ids[i];
@@ -287,13 +299,13 @@ unsigned char dyn2_sync_write_id_present(unsigned char *packet,unsigned char id,
   unsigned char num_servos,i;
   unsigned short int j,len;
 
-  len=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]);
-  num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-7)/(len+1);
+  len=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256;
+  num_servos=((packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-7)/(len+1);
   for(i=0;i<num_servos;i++)
     if(packet[DYN2_DATA_OFF+4+(len+1)*i]==id)
     {
-      (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF]);
-      (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]);
+      (*address)=packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256;
+      (*length)=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256;
       for(j=0;j<(*length);j++)
         data[j]=packet[DYN2_DATA_OFF+4+(len+1)*i+j+1];
       return 0x01;
@@ -311,10 +323,13 @@ void dyn2_init_sync_read_packet(unsigned char *packet,unsigned char num_servos,u
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=0xFE;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=7+num_servos;
+  packet[DYN2_LENGTH_OFF]=(7+num_servos)%256;
+  packet[DYN2_LENGTH_OFF+1]=(7+num_servos)/256;
   packet[DYN2_INST_OFF]=DYN_SYNC_READ;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF])=address;
-  *((unsigned short int *)&packet[DYN2_DATA_OFF+2])=length;
+  packet[DYN2_DATA_OFF]=address%256;
+  packet[DYN2_DATA_OFF+1]=address/256;
+  packet[DYN2_DATA_OFF+2]=length%256;
+  packet[DYN2_DATA_OFF+3]=length/256;
   for(i=0;i<num_servos;i++)
     packet[DYN2_DATA_OFF+4+i]=servo_ids[i];
   dyn2_set_checksum(packet);
@@ -324,12 +339,12 @@ unsigned char dyn2_sync_read_id_present(unsigned char *packet,unsigned char id,u
 {
   unsigned char num_servos,i;
 
-  num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-7);
+  num_servos=(packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256)-7;
   for(i=0;i<num_servos;i++)
     if(packet[DYN2_DATA_OFF+4+i]==id)
     {
-      (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF]);
-      (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+2]);
+      (*address)=packet[DYN2_DATA_OFF]+packet[DYN2_DATA_OFF+1]*256;
+      (*length)=packet[DYN2_DATA_OFF+2]+packet[DYN2_DATA_OFF+3]*256;
       if(i==0)
         return 0x00;
       else
@@ -342,19 +357,24 @@ unsigned char dyn2_sync_read_id_present(unsigned char *packet,unsigned char id,u
 void dyn2_init_bulk_read_packet(unsigned char *packet,unsigned char num_servos,unsigned char *servo_ids,unsigned short int *address,unsigned short int *length)
 {
   unsigned char i;
+  unsigned short int len;
 
   packet[DYN2_HEADER_OFF]=0xFF;
   packet[DYN2_HEADER_OFF+1]=0xFF;
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=0xFE;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=3+5*num_servos;
+  len=3+5*num_servos;
+  packet[DYN2_LENGTH_OFF]=len%256;
+  packet[DYN2_LENGTH_OFF+1]=len/256;
   packet[DYN2_INST_OFF]=DYN_BULK_READ;
   for(i=0;i<num_servos;i++)
   {
     packet[DYN2_DATA_OFF+i*5]=servo_ids[i];
-    *((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+1])=address[i];
-    *((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+3])=length[i];
+    packet[DYN2_DATA_OFF+i*5+1]=address[i]%256;
+    packet[DYN2_DATA_OFF+i*5+2]=address[i]/256;
+    packet[DYN2_DATA_OFF+i*5+3]=length[i]%256;
+    packet[DYN2_DATA_OFF+i*5+4]=length[i]/256;
   }
   dyn2_set_checksum(packet);
 }
@@ -363,12 +383,12 @@ unsigned char dyn2_bulk_read_id_present(unsigned char *packet,unsigned char id,u
 {
   unsigned char num_servos,i,prev_id=0xFF;
 
-  num_servos=(*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-3)/5;
+  num_servos=(packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-3)/5;
   for(i=0;i<num_servos;i++)
     if(packet[DYN2_DATA_OFF+i*5]==id)
     {
-      (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+1]);
-      (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+i*5+3]);
+      (*address)=packet[DYN2_DATA_OFF+i*5+1]+packet[DYN2_DATA_OFF+i*5+2]*256;
+      (*length)=packet[DYN2_DATA_OFF+i*5+3]+packet[DYN2_DATA_OFF+i*5+4]*256;
       if(i==0)
         return 0x00;
       else
@@ -392,15 +412,17 @@ void dyn2_init_bulk_write_packet(unsigned char *packet,unsigned char num_servos,
   packet[DYN2_ID_OFF]=0xFE;
   for(i=0;i<num_servos;i++)
     total_length+=length[i]+5;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=3+total_length;
+  packet[DYN2_LENGTH_OFF]=(3+total_length)%256;
+  packet[DYN2_LENGTH_OFF+1]=(3+total_length)/256;
   packet[DYN2_INST_OFF]=DYN_BULK_WRITE;
   total_length=0;
   for(i=0;i<num_servos;i++)
   {
     packet[DYN2_DATA_OFF+total_length]=servo_ids[i];
-    *((unsigned short int *)&packet[DYN2_DATA_OFF+total_length+1])=address[i];
-    *((unsigned short int *)&packet[DYN2_DATA_OFF+total_length+3])=length[i];
-    packet[DYN2_DATA_OFF+total_length+4]=(length[i]>>8)&0x00FF;
+    packet[DYN2_DATA_OFF+total_length+1]=address[i]%256;
+    packet[DYN2_DATA_OFF+total_length+2]=address[i]/256;
+    packet[DYN2_DATA_OFF+total_length+3]=length[i]%256;
+    packet[DYN2_DATA_OFF+total_length+4]=length[i]/256;
     for(j=0;j<length[i];j++)
       packet[DYN2_DATA_OFF+total_length+5+j]=data[i].data_addr[j];
     total_length+=length[i]+5;
@@ -412,19 +434,19 @@ unsigned char dyn2_bulk_write_id_present(unsigned char *packet,unsigned char id,
 {
   unsigned short int len,offset=0,i;
 
-  len=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-3;
+  len=packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-3;
   while(offset<len)
   {
     if(packet[DYN2_DATA_OFF+offset]==id)
     {
-      (*address)=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+1]);
-      (*length)=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3]);
-      for(i=0;i<*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3]);i++)
+      (*address)=packet[DYN2_DATA_OFF+offset+1]+packet[DYN2_DATA_OFF+offset+2]*256;
+      (*length)=packet[DYN2_DATA_OFF+offset+3]+packet[DYN2_DATA_OFF+offset+4]*256;
+      for(i=0;i<(*length);i++)
         data[i]=packet[DYN2_DATA_OFF+offset+5+i];
       return 0x01;
     }
     else
-      offset+=*((unsigned short int *)&packet[DYN2_DATA_OFF+offset+3])+5;
+      offset+=packet[DYN2_DATA_OFF+offset+3]+packet[DYN2_DATA_OFF+offset+4]*256+5;
   }
 
   return 0x00;
@@ -460,7 +482,8 @@ void dyn2_init_status_packet(unsigned char *packet,unsigned char id,TDynError er
   packet[DYN2_HEADER_OFF+2]=0xFD;
   packet[DYN2_RESERVED]=0x00;
   packet[DYN2_ID_OFF]=id;
-  *((unsigned short int *)&packet[DYN2_LENGTH_OFF])=4+length;
+  packet[DYN2_LENGTH_OFF]=(4+length)%256;
+  packet[DYN2_LENGTH_OFF+1]=(4+length)/256;
   packet[DYN2_INST_OFF]=0x55;
   packet[DYN2_DATA_OFF]=error;
   for(i=0;i<length;i++)
@@ -478,7 +501,7 @@ unsigned short int dyn2_get_read_status_data(unsigned char *packet,unsigned char
 {
   unsigned short int i,length;
 
-  length=*((unsigned short int *)&packet[DYN2_LENGTH_OFF])-4;
+  length=packet[DYN2_LENGTH_OFF]+packet[DYN2_LENGTH_OFF+1]*256-4;
   for(i=0;i<length;i++)
     data[i]=packet[DYN2_DATA_OFF+1+i];
 

dynamixel_base/src/dynamixel_slave.c

@@ -375,6 +375,7 @@ void dyn_v2_slave_loop(TDynamixelSlave *slave)
         case DYN_BULK_WRITE: if(dyn2_bulk_write_id_present(slave->rx_buffer,slave->address,&address,&length,data))
                                error=slave->on_write(address,length,data);
                              break;
+      
         default:
                  break;
       }