diff --git a/servo_firmware/ax12/include/dyn_slave.h b/servo_firmware/ax12/include/dyn_slave.h
index 8d10a99668f2c68256b67176a953f0cad79378c6..99170561f52b897cf6ac4cd3fb19cc956cfe1d79 100644
--- a/servo_firmware/ax12/include/dyn_slave.h
+++ b/servo_firmware/ax12/include/dyn_slave.h
@@ -11,11 +11,10 @@
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
-#define INST_DIGITAL_RESET 0x07
-#define INST_SYSTEM_READ 0x0C
-#define INST_SYSTEM_WRITE 0x0D
+#define INST_SYNC_READ 0x82
#define INST_SYNC_WRITE 0x83
-#define INST_SYNC_REG_WRITE 0x84
+#define INST_BULK_READ 0x92
+#define INST_BULK_WRITE 0x93
// bus errors
#define INSTRUCTION_ERROR 0x40
@@ -34,10 +33,21 @@ uint8_t dyn_slave_is_packet_ready(void);
uint8_t dyn_slave_check_id(void);
uint8_t dyn_slave_check_checksum(void);
uint8_t dyn_slave_get_instruction(void);
+uint8_t dyn_slave_get_received_id(void);
+// read operation
uint8_t dyn_slave_get_address(void);
uint8_t dyn_slave_get_read_length(void);
+// write operation
uint8_t dyn_slave_get_write_length(void);
uint8_t *dyn_slave_get_write_data(void);
+// sync read operation
+uint8_t dyn_slave_sync_read_id_present(void);
+// sync write operation
+uint8_t dyn_slave_sync_write_id_present(uint8_t **data);
+// bulk read operation
+uint8_t dyn_slave_bulk_read_id_present(uint8_t *address,uint8_t *length);
+// bulk write operation
+uint8_t dyn_slave_bulk_write_id_present(uint8_t *address,uint8_t *length,uint8_t **data);
void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data);
uint8_t dyn_slave_is_send_done(void);
diff --git a/servo_firmware/ax12/include/mem.h b/servo_firmware/ax12/include/mem.h
index aad100ccd8346653a77931825a937c3e987e44aa..b39ab683f5939153e6e31f3cd70a7954a49a61cb 100644
--- a/servo_firmware/ax12/include/mem.h
+++ b/servo_firmware/ax12/include/mem.h
@@ -51,7 +51,9 @@
#define KD_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
#define KD_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
-extern unsigned char ram_data[50];
+#define RAM_SIZE 50
+
+extern unsigned char ram_data[RAM_SIZE];
void ram_init(void);
uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data);
diff --git a/servo_firmware/ax12/src/comm.c b/servo_firmware/ax12/src/comm.c
index bde386a84f7f6d2a4148af68f05b5c69a8b5b9c7..eef884ae0f30c6b89214cb295ee8225943576f79 100644
--- a/servo_firmware/ax12/src/comm.c
+++ b/servo_firmware/ax12/src/comm.c
@@ -4,6 +4,14 @@
#include "gpio.h"
uint8_t dyn_error;
+uint8_t tmp_data[RAM_SIZE];
+uint8_t tmp_address;
+uint8_t tmp_length;
+uint8_t reg_pending;
+uint8_t sync_read_pending;
+uint8_t sync_read_previous;
+uint8_t bulk_read_pending;
+uint8_t bulk_read_previous;
void do_write(uint8_t address,uint8_t length,uint8_t *data)
{
@@ -28,11 +36,16 @@ void do_write(uint8_t address,uint8_t length,uint8_t *data)
void comm_init(void)
{
dyn_error=NO_ERROR;
+ tmp_address=0x00;
+ tmp_length=0x00;
+ reg_pending=0x00;
+ sync_read_pending=0x00;
+ bulk_read_pending=0x00;
}
void comm_loop(void)
{
- uint8_t *data;
+ static uint8_t *data,i;
if(dyn_slave_is_packet_ready())
{
@@ -57,6 +70,71 @@ void comm_loop(void)
else
dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
break;
+ case INST_REG_WRITE: tmp_address=dyn_slave_get_address();
+ tmp_length=dyn_slave_get_write_length();
+ data=dyn_slave_get_write_data();
+ for(i=0;i<tmp_length;i++)
+ tmp_data[i]=data[i];
+ reg_pending=0x01;
+ dyn_slave_send_status(dyn_error|NO_ERROR,0x00,0x00000000);
+ break;
+ case INST_ACTION: if(reg_pending)
+ {
+ if(ram_write(tmp_address,tmp_length,tmp_data))
+ {
+ do_write(tmp_address,tmp_length,tmp_data);
+ dyn_slave_send_status(dyn_error|NO_ERROR,0x00,0x00000000);
+ }
+ }
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ case INST_RESET:
+ break;
+ case INST_SYNC_READ: sync_read_previous=dyn_slave_sync_read_id_present();
+ if(sync_read_previous!=0xFF)
+ {
+ if(sync_read_previous==0x00)// first device on the list
+ {
+ if(ram_read(dyn_slave_get_address(),dyn_slave_get_read_length(),&data))
+ dyn_slave_send_status(dyn_error|NO_ERROR,dyn_slave_get_read_length(),data);
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ else// sync read pending
+ {
+ tmp_address=dyn_slave_get_address();
+ tmp_length=dyn_slave_get_read_length();
+ sync_read_pending=0x01;
+ }
+ }
+ break;
+ case INST_SYNC_WRITE: if(dyn_slave_sync_write_id_present(&data))
+ {
+ if(ram_write(dyn_slave_get_address(),dyn_slave_get_read_length(),data))
+ do_write(dyn_slave_get_address(),dyn_slave_get_read_length(),data);
+ }
+ break;
+ case INST_BULK_READ: bulk_read_previous=dyn_slave_bulk_read_id_present(&tmp_address,&tmp_length);
+ if(bulk_read_previous!=0xFF)
+ {
+ if(bulk_read_previous==0x00)// first device on the list
+ {
+ if(ram_read(tmp_address,tmp_length,&data))
+ dyn_slave_send_status(dyn_error|NO_ERROR,tmp_length,data);
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ else// sync read pending
+ bulk_read_pending=0x01;
+ }
+ break;
+ case INST_BULK_WRITE: if(dyn_slave_bulk_write_id_present(&tmp_address,&tmp_length,&data))
+ {
+ if(ram_write(tmp_address,tmp_length,data))
+ do_write(tmp_address,tmp_length,data);
+ }
+ break;
default: dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
break;
}
@@ -64,6 +142,31 @@ void comm_loop(void)
else
dyn_slave_send_status(dyn_error|CHECKSUM_ERROR,0x00,0x00000000);
}
+ else
+ {
+ if(sync_read_pending)
+ {
+ if(dyn_slave_get_received_id()==sync_read_previous)
+ {
+ sync_read_pending=0x00;
+ if(ram_read(tmp_address,tmp_length,&data))
+ dyn_slave_send_status(dyn_error|NO_ERROR,tmp_length,data);
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ }
+ if(bulk_read_pending)
+ {
+ if(dyn_slave_get_received_id()==bulk_read_previous)
+ {
+ bulk_read_pending=0x00;
+ if(ram_read(tmp_address,tmp_length,&data))
+ dyn_slave_send_status(dyn_error|NO_ERROR,tmp_length,data);
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ }
+ }
}
}
diff --git a/servo_firmware/ax12/src/dyn_slave.c b/servo_firmware/ax12/src/dyn_slave.c
index a43a39fdde5d2316bf8fa9a3676289ccae85a392..66b93b8f3c20a044bb0cc67479eb7e84da65e784 100644
--- a/servo_firmware/ax12/src/dyn_slave.c
+++ b/servo_firmware/ax12/src/dyn_slave.c
@@ -26,7 +26,6 @@ ISR(USART_TXC_vect)
{
if(dyn_slave_num_tx_bytes==dyn_slave_data[3]+4)
{
- dyn_slave_num_rx_bytes=0;
dyn_slave_set_rx();
dyn_slave_send_done=1;
}
@@ -114,7 +113,7 @@ uint8_t dyn_slave_is_packet_ready(void)
uint8_t dyn_slave_check_id(void)
{
- if(dyn_slave_id==dyn_slave_data[2])
+ if(dyn_slave_id==dyn_slave_data[2] || dyn_slave_data[2]==0xFE)
return 0x01;
else
{
@@ -140,6 +139,12 @@ uint8_t dyn_slave_get_instruction(void)
return dyn_slave_data[4];
}
+uint8_t dyn_slave_get_received_id(void)
+{
+ return dyn_slave_data[2];
+}
+
+// read operation
uint8_t dyn_slave_get_address(void)
{
return dyn_slave_data[5];
@@ -150,6 +155,7 @@ uint8_t dyn_slave_get_read_length(void)
return dyn_slave_data[6];
}
+// write operation
uint8_t dyn_slave_get_write_length(void)
{
return dyn_slave_data[3]-3;
@@ -160,6 +166,88 @@ uint8_t *dyn_slave_get_write_data(void)
return &dyn_slave_data[6];
}
+// sync read operation
+uint8_t dyn_slave_sync_read_id_present(void)
+{
+ uint8_t i,num;
+
+ num=dyn_slave_num_rx_bytes-8;
+ dyn_slave_num_rx_bytes=0;
+ for(i=0;i<num;i++)
+ {
+ if(dyn_slave_data[7+i]==dyn_slave_id)
+ {
+ if(i==0)
+ return 0x00;
+ else
+ return dyn_slave_data[6+i];
+ }
+ }
+ return 0xFF;
+}
+
+// sync write operation
+uint8_t dyn_slave_sync_write_id_present(uint8_t **data)
+{
+ uint8_t i,num;
+
+ num=(dyn_slave_num_rx_bytes-8)/(dyn_slave_data[6]+1);
+ dyn_slave_num_rx_bytes=0;
+ for(i=0;i<num;i++)
+ {
+ if(dyn_slave_data[7+i*(dyn_slave_data[6]+1)]==dyn_slave_id)
+ {
+ (*data)=&dyn_slave_data[7+i*(dyn_slave_data[6]+1)+1];
+ return 0x01;
+ }
+ }
+ return 0x00;
+}
+
+// bulk read operation
+uint8_t dyn_slave_bulk_read_id_present(uint8_t *address,uint8_t *length)
+{
+ uint8_t i,num;
+
+ num=(dyn_slave_num_rx_bytes-6)/3;
+ dyn_slave_num_rx_bytes=0;
+ for(i=0;i<num;i++)
+ {
+ if(dyn_slave_data[5+i*3]==dyn_slave_id)
+ {
+ (*address)=dyn_slave_data[6+i*3];
+ (*length)=dyn_slave_data[7+i*3];
+ if(i==0)
+ return 0x00;
+ else
+ return dyn_slave_data[5+(i-1)*3];
+ }
+ }
+ return 0xFF;
+}
+
+// bulk write operation
+uint8_t dyn_slave_bulk_write_id_present(uint8_t *address,uint8_t *length,uint8_t **data)
+{
+ uint8_t i,num;
+
+ num=dyn_slave_num_rx_bytes;
+ dyn_slave_num_rx_bytes=0;
+ for(i=5;i<num;)
+ {
+ if(dyn_slave_data[i]==dyn_slave_id)
+ {
+ (*address)=dyn_slave_data[i+1];
+ (*length)=dyn_slave_data[i+2];
+ (*data)=&dyn_slave_data[i+3];
+ return 0x01;
+ }
+ else
+ i+=dyn_slave_data[i+2]+3;
+ }
+ return 0x00;
+}
+
void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
{
uint8_t i,cs=0;