1782 fsm->change_state = ec_fsm_change_error; |
1782 fsm->change_state = ec_fsm_change_error; |
1783 EC_ERR("Reception of state ack datagram failed.\n"); |
1783 EC_ERR("Reception of state ack datagram failed.\n"); |
1784 return; |
1784 return; |
1785 } |
1785 } |
1786 |
1786 |
|
1787 fsm->change_start = get_cycles(); |
|
1788 |
1787 // read new AL status |
1789 // read new AL status |
1788 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
1790 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
1789 ec_master_queue_datagram(fsm->master, datagram); |
1791 ec_master_queue_datagram(fsm->master, datagram); |
1790 fsm->change_state = ec_fsm_change_ack2; |
1792 fsm->change_state = ec_fsm_change_ack2; |
1791 } |
1793 } |
1799 |
1801 |
1800 void ec_fsm_change_ack2(ec_fsm_t *fsm /**< finite state machine */) |
1802 void ec_fsm_change_ack2(ec_fsm_t *fsm /**< finite state machine */) |
1801 { |
1803 { |
1802 ec_datagram_t *datagram = &fsm->datagram; |
1804 ec_datagram_t *datagram = &fsm->datagram; |
1803 ec_slave_t *slave = fsm->slave; |
1805 ec_slave_t *slave = fsm->slave; |
|
1806 ec_slave_state_t ack_state; |
1804 |
1807 |
1805 if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { |
1808 if (datagram->state != EC_CMD_RECEIVED || datagram->working_counter != 1) { |
1806 fsm->change_state = ec_fsm_change_error; |
1809 fsm->change_state = ec_fsm_change_error; |
1807 EC_ERR("Reception of state ack check datagram failed.\n"); |
1810 EC_ERR("Reception of state ack check datagram failed.\n"); |
1808 return; |
1811 return; |
1809 } |
1812 } |
1810 |
1813 |
1811 slave->current_state = EC_READ_U8(datagram->data); |
1814 ack_state = EC_READ_U8(datagram->data); |
1812 |
1815 |
1813 if (slave->current_state == fsm->change_new) { |
1816 if (ack_state == slave->current_state) { |
1814 fsm->change_state = ec_fsm_change_error; |
1817 fsm->change_state = ec_fsm_change_error; |
1815 EC_INFO("Acknowleged state 0x%02X on slave %i.\n", |
1818 EC_INFO("Acknowleged state 0x%02X on slave %i.\n", |
1816 slave->current_state, slave->ring_position); |
1819 slave->current_state, slave->ring_position); |
1817 return; |
1820 return; |
1818 } |
1821 } |
1819 |
1822 |
1820 fsm->change_state = ec_fsm_change_error; |
1823 if (get_cycles() - fsm->change_start >= (cycles_t) 100 * cpu_khz) { |
1821 EC_WARN("Failed to acknowledge state 0x%02X on slave %i" |
1824 // timeout while checking |
1822 " - Timeout!\n", fsm->change_new, slave->ring_position); |
1825 slave->current_state = EC_SLAVE_STATE_UNKNOWN; |
|
1826 fsm->change_state = ec_fsm_change_error; |
|
1827 EC_ERR("Timeout while acknowleging state 0x%02X on slave %i.\n", |
|
1828 fsm->change_new, slave->ring_position); |
|
1829 return; |
|
1830 } |
|
1831 |
|
1832 // reread new AL status |
|
1833 ec_datagram_nprd(datagram, slave->station_address, 0x0130, 2); |
|
1834 ec_master_queue_datagram(fsm->master, datagram); |
1823 } |
1835 } |
1824 |
1836 |
1825 /*****************************************************************************/ |
1837 /*****************************************************************************/ |
1826 |
1838 |
1827 /** |
1839 /** |