From 5f754a57410b4d0d77f56a3ef7fb45013baba7ea Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 22:46:06 +0200 Subject: digital/io-hub/src/apbirthday: drop messages for non essential I2C bus --- digital/io-hub/src/apbirthday/robot.cc | 2 +- digital/io-hub/src/common-cc/i2c_queue.cc | 10 +++++++--- digital/io-hub/src/common-cc/i2c_queue.hh | 4 +++- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/digital/io-hub/src/apbirthday/robot.cc b/digital/io-hub/src/apbirthday/robot.cc index 2ca626aa..754db53a 100644 --- a/digital/io-hub/src/apbirthday/robot.cc +++ b/digital/io-hub/src/apbirthday/robot.cc @@ -37,7 +37,7 @@ Robot *robot; Robot::Robot () : main_i2c_queue_ (hardware.main_i2c), - secondary_i2c_queue_ (hardware.secondary_i2c), + secondary_i2c_queue_ (hardware.secondary_i2c, true), zb_i2c_queue_ (hardware.zb_i2c), asserv (main_i2c_queue_, BOT_SCALE), mimot (main_i2c_queue_), diff --git a/digital/io-hub/src/common-cc/i2c_queue.cc b/digital/io-hub/src/common-cc/i2c_queue.cc index 0e418241..0e554fbd 100644 --- a/digital/io-hub/src/common-cc/i2c_queue.cc +++ b/digital/io-hub/src/common-cc/i2c_queue.cc @@ -62,8 +62,8 @@ I2cQueue::Slave::seq_next () return seq_; } -I2cQueue::I2cQueue (ucoo::I2cMaster &i2c) - : i2c_ (i2c), slaves_ (0), update_state_ (IDLE), +I2cQueue::I2cQueue (ucoo::I2cMaster &i2c, bool can_drop) + : i2c_ (i2c), can_drop_ (can_drop), slaves_ (0), update_state_ (IDLE), queue_head_ (0), queue_tail_ (0), queue_timeout_ (0) { i2c_.register_finished (*this); @@ -128,7 +128,11 @@ I2cQueue::send (Slave &slave, const uint8_t *command, int size, } else { - ucoo::assert (queue_head_ != queue_next (queue_tail_)); + if (queue_head_ == queue_next (queue_tail_)) + { + ucoo::assert (can_drop_); + return; + } c = &queue_[queue_tail_]; } // Fill and copy data. diff --git a/digital/io-hub/src/common-cc/i2c_queue.hh b/digital/io-hub/src/common-cc/i2c_queue.hh index 356b3a2a..42823b2a 100644 --- a/digital/io-hub/src/common-cc/i2c_queue.hh +++ b/digital/io-hub/src/common-cc/i2c_queue.hh @@ -124,7 +124,7 @@ class I2cQueue : public ucoo::I2cMaster::FinishedHandler }; public: /// Constructor. - I2cQueue (ucoo::I2cMaster &i2c); + I2cQueue (ucoo::I2cMaster &i2c, bool can_drop = false); /// Synchronise all slaves, return true if synchronised (no message in /// queue). bool sync (); @@ -150,6 +150,8 @@ class I2cQueue : public ucoo::I2cMaster::FinishedHandler private: /// I2C interface. ucoo::I2cMaster &i2c_; + /// Can drop reliable command in case of big problem. + bool can_drop_; /// Chained list of slaves. Slave *slaves_; /// State of update FSM. -- cgit v1.2.3