[paparazzi-commits] [4954]

[antoine drouin]

Revision: 4954
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4954
Author:   poine
Date:     2010-06-14 16:58:05 +0000 (Mon, 14 Jun 2010)
Log Message:
-----------


Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/motor_bench/main_turntable.c
    paparazzi3/trunk/sw/airborne/motor_bench/turntable_systime.c

Added: paparazzi3/trunk/sw/airborne/motor_bench/main_turntable.c
===================================================================
--- paparazzi3/trunk/sw/airborne/motor_bench/main_turntable.c                   
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/motor_bench/main_turntable.c   2010-06-14 
16:58:05 UTC (rev 4954)
@@ -0,0 +1,102 @@
+#include "std.h"
+#include "init_hw.h"
+#include "sys_time.h"
+#include "led.h"
+#include "interrupt_hw.h"
+#include "usb_serial.h"
+
+#include "messages.h"
+#include "downlink.h"
+
+#include "armVIC.h"
+
+
+static inline void main_init( void );
+static inline void main_periodic( void );
+static inline void main_event( void );
+
+static inline void main_init_tacho(void);
+static uint32_t lp_pulse;
+static uint32_t nb_pulse = 0;
+static float omega_rad;
+
+
+int main( void ) {
+  main_init();
+  while(1) {
+    if (sys_time_periodic())
+      main_periodic();
+    main_event();
+  }
+  return 0;
+}
+
+static inline void main_init( void ) {
+  hw_init();
+  sys_time_init();
+  main_init_tacho();
+  int_enable();
+}
+
+#define NB_STEP 256
+static inline void main_periodic( void ) {
+
+  RunOnceEvery(50, {
+      const float tach_to_rpm = 15000000.*2*M_PI/(float)NB_STEP;
+      omega_rad = tach_to_rpm / lp_pulse;
+      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, &omega_rad);}
+      //      float foo = nb_pulse;
+      //      DOWNLINK_SEND_IMU_TURNTABLE(DefaultChannel, &foo);}
+    );
+  RunOnceEvery(100, {DOWNLINK_SEND_ALIVE(DefaultChannel,  16, MD5SUM);});
+
+}
+
+
+static inline void main_event( void ) {
+
+}
+
+
+
+
+/* INPUT CAPTURE CAP0.0 on P0.22*/
+#define TT_TACHO_PINSEL     PINSEL1
+#define TT_TACHO_PINSEL_VAL 0x02
+#define TT_TACHO_PINSEL_BIT 12
+
+static inline void main_init_tacho(void) {
+  /* select pin for capture */
+  TT_TACHO_PINSEL |= TT_TACHO_PINSEL_VAL << TT_TACHO_PINSEL_BIT;
+  /* enable capture 0.2 on falling edge + trigger interrupt */
+  T0CCR |= TCCR_CR0_F | TCCR_CR0_I;
+}
+
+
+//
+//  trimed version of arm7/sys_time_hw.c
+//
+
+uint32_t cpu_time_ticks;
+uint32_t last_periodic_event;
+
+uint32_t sys_time_chrono_start; /* T0TC ticks */
+uint32_t sys_time_chrono;       /* T0TC ticks */
+
+
+void TIMER0_ISR ( void ) {
+  ISR_ENTRY();
+  //  LED_TOGGLE(1);
+  if (T0IR & TIR_CR0I) {
+    static uint32_t pulse_last_t;
+    uint32_t t_now = T0CR0;
+    uint32_t diff = t_now - pulse_last_t;
+    lp_pulse = (lp_pulse + diff)/2;
+    pulse_last_t = t_now;
+    nb_pulse++;
+    //    got_one_pulse = TRUE;
+    T0IR = TIR_CR0I;
+  }
+  VICVectAddr = 0x00000000;
+  ISR_EXIT();
+}

Added: paparazzi3/trunk/sw/airborne/motor_bench/turntable_systime.c
===================================================================
--- paparazzi3/trunk/sw/airborne/motor_bench/turntable_systime.c                
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/motor_bench/turntable_systime.c        
2010-06-14 16:58:05 UTC (rev 4954)
@@ -0,0 +1,3 @@
+#include "armVIC.h"
+#include "sys_time.h"
+