[paparazzi-commits] [4341] Update mercury_csc to support sending motors in sequence after timeout ( instead of waiting for previous

[Allen Ibara]

Revision: 4341
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4341
Author:   aibara
Date:     2009-11-19 18:39:37 +0000 (Thu, 19 Nov 2009)
Log Message:
-----------
Update mercury_csc to support sending motors in sequence after timeout (instead 
of waiting for previous
transfer to complete).

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
    paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
    paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c

Modified: paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c    2009-11-19 
18:38:33 UTC (rev 4340)
+++ paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.c    2009-11-19 
18:39:37 UTC (rev 4341)
@@ -1,12 +1,22 @@
 #include "buss_twi_blmc_hw.h"
 #include <inttypes.h>
 
+// From sys_time_hw.h
+#define T0_PCLK_DIV 1
+
+// Number of PCLK cycles to wait between sending commands
+#define TICS_OF_BLMC_IDX (I2C_CYCLES_PER_MSG * (I2C0_SCLL + I2C0_SCLH) / 
T0_PCLK_DIV)
+
+// Number of I2C cycles to wait before timeout and send next command 
(including some margin)
+#define I2C_CYCLES_PER_MSG 48
+
 uint8_t twi_blmc_nb_err;
 
 uint8_t motor_power[BUSS_TWI_BLMC_NB];
 static volatile bool_t  buss_twi_blmc_status;
 static volatile bool_t  buss_twi_blmc_i2c_done;
 static volatile uint8_t buss_twi_blmc_idx;
+static uint32_t commit_start_time;
 
 const uint8_t buss_twi_blmc_addr[BUSS_TWI_BLMC_NB] = BUSS_BLMC_ADDR;
 
@@ -39,16 +49,27 @@
 
 void motors_commit(int force)
 {
+
   if (force || buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_IDLE) {
+    I2c0SendStop();
     buss_twi_blmc_idx = 0;                                             
     buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_BUSY;                  
     buss_twi_blmc_send_next();
+
+    commit_start_time = T0TC;
+
   }
 }
 
 
-void motors_commit_next()
+void motors_callback_nop()
 {
+  // do nothing...
+  // We don't use this to kick the next command automatically anymore
+}
+
+static void motors_commit_next()
+{
     buss_twi_blmc_idx++;                               
     if (buss_twi_blmc_idx < BUSS_TWI_BLMC_NB_SEND)
       buss_twi_blmc_send_next();
@@ -56,8 +77,18 @@
       buss_twi_blmc_status = BUSS_TWI_BLMC_STATUS_IDLE;        
 }
 
-
-
-
-
-
+void motors_event( void )
+{
+  // if busy sending, check progress
+  if (buss_twi_blmc_status == BUSS_TWI_BLMC_STATUS_BUSY) {
+    // check timer 0 counter to see if it is time for next message
+    if ((T0TC - commit_start_time) > TICS_OF_BLMC_IDX) {
+      // abort any previous activity
+      I2c0SendStop();
+      // advance index and start transmit on next message
+      motors_commit_next();
+      // record current time
+      commit_start_time = T0TC;
+    }
+  }
+}

Modified: paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h    2009-11-19 
18:38:33 UTC (rev 4340)
+++ paparazzi3/trunk/sw/airborne/csc/arm7/buss_twi_blmc_hw.h    2009-11-19 
18:39:37 UTC (rev 4341)
@@ -15,6 +15,7 @@
 void motors_init ( void );
 void motors_set_motor(uint8_t id, int16_t value);
 void motors_commit(int force);
-void motors_commit_next( void );
+void motors_callback_nop( void );
+void motors_event( void );
 
 #endif /* BUSS_TWI_BLMC_HW_H */

Modified: paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2009-11-19 18:38:33 UTC 
(rev 4340)
+++ paparazzi3/trunk/sw/airborne/csc/mercury_csc_main.c 2009-11-19 18:39:37 UTC 
(rev 4341)
@@ -179,6 +179,9 @@
 static void csc_main_event( void ) {
 
   csc_can_event();
+#ifdef USE_BUSS_TWI_BLMC_MOTOR
+  motors_event();
+#endif
 #ifdef SPEKTRUM_LINK
   spektrum_event_task();
 #endif