motor-control: Added LED debugging to measure timing

diff --git a/src/motor-control/brushless.c b/src/motor-control/brushless.c
index bea2a003d81851a64bfc8375cfb12d7df2eea86f..42a2b90040972030058fcdf05e85aea7037e74ab 100644 (file)
--- a/src/motor-control/brushless.c
+++ b/src/motor-control/brushless.c
@@ -32,12 +32,15 @@
  * @name Motor control (h8eurobot)
  * @{
  */
-#define LED_LIVE   0           /**< Blinks once per second when software is alive */
-#define LED_CAN_REC 1          /**< Blinks when a CAN message is received:
+/* #define LED_ODO_SEND 0 */
+/* #define LED_CORR_TRIG 1 */
+/* #define LED_MAIN_LOOP 2 */
+#define LED_LIVE   0           /**< D1: Blinks once per second when software is alive */
+#define LED_CAN_REC 1          /**< D2: Blinks when a CAN message is received:
                                 * - 2ms for unknown message
                                 * - 100ms for motion message */
-#define LED_ERROR  2           /**< Light: Unhandled exception, Slow blink (4s): motor error */
-#define LED_RESET  3           /**< Blinks for 1 s after reset */
+#define LED_ERROR  2           /**< D3: Light = Unhandled exception, Slow blink (4s) = motor error */
+#define LED_RESET  3           /**< D4: Blinks for 1 s after reset */
 /** @} */
 /** @} */
 
@@ -367,10 +370,12 @@ cmd_des_t const **cmd_list=cmd_list_default;     /*cmd prikazy pro PC*/
 void  unhandled_exception(void) __attribute__ ((interrupt_handler));
 void  unhandled_exception(void)
 {
+#ifdef LED_ERROR
     DEB_LED_ON(LED_ERROR);
     FlWait(20000);
     DEB_LED_OFF(LED_ERROR);
     FlWait(20000);
+#endif
 }
 
 void print_mcs_err(pxmc_state_t *mcs, char *name)
@@ -413,6 +418,17 @@ static inline short sendOdometry()
 }
 #endif
 
+
+static inline void blink_odo_send(void)
+{
+#ifdef LED_ODO_SEND
+    static bool led;
+    if (led) DEB_LED_ON(LED_ODO_SEND);
+    else DEB_LED_OFF(LED_ODO_SEND);
+    led = !led;
+#endif
+}
+
 /** 
  * Sends only pxms_ap. Odometry is calculated elsewhere and since we
  * don't send differences, sequence numbers are not necessary.
@@ -426,6 +442,7 @@ static inline void sendOdometrySimple()
     lap = mcs_left.pxms_ap;
     rap = mcs_right.pxms_ap;
 
+    blink_odo_send();
 
     m.cob_id.w = CAN_MOTION_ODOMETRY_SIMPLE;
     m.len = 8;
@@ -452,9 +469,18 @@ static inline short storeOdometryInTable()
 #endif
 
 
+static inline void blink_err_led(void)
+{
+#ifdef LED_ERROR
+       static bool err_led = false;
+       err_led = !err_led;
+       if (err_led) DEB_LED_ON(LED_ERROR);
+       else DEB_LED_OFF(LED_ERROR);
+#endif
+}
+
 static inline void handle_motor_errors() {
     static unsigned last_msg_time=0;
-    static bool err_led = false;
     int i;
     pxmc_state_t *mcs;
     
@@ -462,9 +488,7 @@ static inline void handle_motor_errors() {
         pxmc_stop(&mcs_left, 0);
         pxmc_stop(&mcs_right, 0);
         if (pxmc_msec_counter - last_msg_time >= 2000) {
-           err_led = !err_led;
-           if (err_led) DEB_LED_ON(LED_ERROR);
-           else DEB_LED_OFF(LED_ERROR);
+               blink_err_led();
             last_msg_time = pxmc_msec_counter;
             if (mcs_left.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_left, "L");
             if (mcs_right.pxms_flg&PXMS_ERR_m) print_mcs_err(&mcs_right, "R");
@@ -506,6 +530,16 @@ void led_can_rec(unsigned duration_msec)
 #endif
 }
 
+static inline void blink_corr_trig(void)
+{
+#ifdef LED_CORR_TRIG
+       static bool led;
+       if (led) DEB_LED_ON(LED_CORR_TRIG);
+       else DEB_LED_OFF(LED_CORR_TRIG);
+       led = !led;
+#endif
+}
+
 
 static inline void handle_can_receive(void) 
 {
@@ -536,6 +570,7 @@ static inline void handle_can_receive(void)
 
            case CAN_CORR_TRIG:
                odometry_triggered = 1;
+               blink_corr_trig();
                break;
         }
 
@@ -589,7 +624,18 @@ static inline void handle_status_send()
     }
 }
 
-void handle_leds()
+static inline void blink_main_loop()
+{
+#ifdef LED_MAIN_LOOP
+       static bool led = false;
+       led = !led;
+       if (led) DEB_LED_ON(LED_MAIN_LOOP);
+       else DEB_LED_OFF(LED_MAIN_LOOP);
+#endif
+
+}
+
+static inline void handle_leds()
 {
 #define PERIOD 1000
        static unsigned last=0;
@@ -597,15 +643,23 @@ void handle_leds()
         if (pxmc_msec_counter-last >= PERIOD/2) {
                last += PERIOD/2;
                on = !on;
-               if (on)
+               if (on) {
+#ifdef LED_LIVE
                        DEB_LED_ON(LED_LIVE);
-               else {
+#endif                 
+               } else {
+#ifdef LED_LIVE
                        DEB_LED_OFF(LED_LIVE);
+#endif
+#ifdef LED_RESET
                        DEB_LED_OFF(LED_RESET);
+#endif
                }
        }
         
 #undef PERIOD
+
+       blink_main_loop();
 }
 
 void _print(char *ptr);