diff --git a/src/dyn_slave_diff.c b/src/dyn_slave_diff.c
index b0cee4c8ca33777b5f29b1e0b7acc729b09bed99..3fef1531645b993742b946b72c0f3115d5d14724 100644
--- a/src/dyn_slave_diff.c
+++ b/src/dyn_slave_diff.c
@@ -3,6 +3,7 @@
#include <avr/io.h>
#include "dyn_slave_diff.h"
#include "mem.h"
+#include "pwm_dyn_emm.h"
#ifndef DYN_SLAVE_DIFF_BUFFER_LEN
#define DYN_SLAVE_DIFF_BUFFER_LEN 128
@@ -106,6 +107,11 @@ uint8_t dyn_slave_diff_check_id(void)
}
}
+uint8_t dyn_slave_diff_get_id(void)
+{
+ return dyn_slave_diff_data[2];
+}
+
uint8_t dyn_slave_diff_check_checksum(void)
{
uint8_t i,cs=0x00;
@@ -239,8 +245,8 @@ void dyn_slave_diff_send_status(uint8_t error,uint8_t length, uint8_t *data)
ram_data[Status_Return_Level]!=0x00) || ram_data[Status_Return_Level]==0x02)
dyn_slave_diff_data[0]=0xFF;
dyn_slave_diff_data[1]=0xFF;
- dyn_slave_diff_data[2]=dyn_slave_diff_id;
- cs+=dyn_slave_diff_id;
+ dyn_slave_diff_data[2]=dyn_slave_diff_get_id();
+ cs+=dyn_slave_diff_get_id();
dyn_slave_diff_data[3]=length+2;
cs+=length+2;
dyn_slave_diff_data[4]=error;
@@ -299,20 +305,24 @@ void dyn_slave_diff_set_baudrate(uint8_t baudrate)
{
UBRR0H = 0;
UBRR0L = baudrate;
+ ram_data[Baud_Rate]=baudrate;
}
void dyn_slave_diff_set_id(uint8_t id)
{
dyn_slave_diff_id=id;
+ ram_data[ID]=id;
}
void dyn_slave_diff_loop(void)
{
static uint8_t *data,i;
+ uint8_t id;
if(dyn_slave_diff_is_packet_ready())
- {
- if(dyn_slave_diff_check_id())
+ {
+ id=dyn_slave_diff_get_id();
+ if(dyn_slave_diff_check_id() || pwm_dyn_check_id(id))
{
if(dyn_slave_diff_check_checksum())
{
@@ -320,28 +330,55 @@ void dyn_slave_diff_loop(void)
{
case INST_PING: dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
break;
- case INST_READ: if(do_read(dyn_slave_diff_get_address(),dyn_slave_diff_get_read_length(),&data))
- dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,dyn_slave_diff_get_read_length(),data);
- else
- dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ case INST_READ: if(dyn_slave_diff_check_id())
+ {
+ if(do_read(dyn_slave_diff_get_address(),dyn_slave_diff_get_read_length(),&data))
+ dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,dyn_slave_diff_get_read_length(),data);
+ else
+ dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ if(pwm_dyn_check_id(id))
+ {
+ if(pwm_dyn_do_read(id,dyn_slave_diff_get_address(),dyn_slave_diff_get_read_length(),&data))
+ dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,dyn_slave_diff_get_read_length(),data);
+ else
+ dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
break;
- case INST_WRITE: if(do_write(dyn_slave_diff_get_address(),dyn_slave_diff_get_write_length(),dyn_slave_diff_get_write_data()))
- dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
- else
- dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ case INST_WRITE: if(dyn_slave_diff_check_id())
+ {
+ if(do_write(dyn_slave_diff_get_address(),dyn_slave_diff_get_write_length(),dyn_slave_diff_get_write_data()))
+ dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
+ else
+ dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
+ if(pwm_dyn_check_id(id))
+ {
+ if(pwm_dyn_do_write(id,dyn_slave_diff_get_address(),dyn_slave_diff_get_write_length(),dyn_slave_diff_get_write_data()))
+ dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
+ else
+ dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ }
break;
- case INST_REG_WRITE: dyn_slave_diff_tmp_address=dyn_slave_diff_get_address();
- dyn_slave_diff_tmp_length=dyn_slave_diff_get_write_length();
- data=dyn_slave_diff_get_write_data();
- for(i=0;i<dyn_slave_diff_tmp_length;i++)
- dyn_slave_diff_tmp_data[i]=data[i];
- dyn_slave_diff_reg_pending=0x01;
+ case INST_REG_WRITE: if(dyn_slave_diff_check_id())
+ {
+ dyn_slave_diff_tmp_address=dyn_slave_diff_get_address();
+ dyn_slave_diff_tmp_length=dyn_slave_diff_get_write_length();
+ data=dyn_slave_diff_get_write_data();
+ for(i=0;i<dyn_slave_diff_tmp_length;i++)
+ dyn_slave_diff_tmp_data[i]=data[i];
+ dyn_slave_diff_reg_pending=0x01;
+ }
+ if(pwm_dyn_check_id(id))
+ pwm_dyn_do_reg_write(id,dyn_slave_diff_get_address(),dyn_slave_diff_get_write_length(),dyn_slave_diff_get_write_data());
dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
break;
case INST_ACTION: if(dyn_slave_diff_reg_pending)
{
- if(do_write(dyn_slave_diff_tmp_address,dyn_slave_diff_tmp_length,dyn_slave_diff_tmp_data))
- dyn_slave_diff_send_status(dyn_slave_diff_error|NO_ERROR,0x00,0x00000000);
+ if(dyn_slave_diff_check_id())
+ do_write(dyn_slave_diff_tmp_address,dyn_slave_diff_tmp_length,dyn_slave_diff_tmp_data);
+ if(pwm_dyn_check_id(id))
+ pwm_dyn_do_action(id);
}
else
dyn_slave_diff_send_status(dyn_slave_diff_error|INSTRUCTION_ERROR,0x00,0x00000000);
@@ -368,6 +405,7 @@ void dyn_slave_diff_loop(void)
break;
case INST_SYNC_WRITE: if(dyn_slave_diff_sync_write_id_present(&data))
do_write(dyn_slave_diff_get_address(),dyn_slave_diff_get_read_length(),data);
+ pwm_dyn_do_sync_write(dyn_slave_diff_data);
break;
case INST_BULK_READ: dyn_slave_diff_bulk_read_previous=dyn_slave_diff_bulk_read_id_present(&dyn_slave_diff_tmp_address,&dyn_slave_diff_tmp_length);
if(dyn_slave_diff_bulk_read_previous!=0xFF)