*** empty log message ***

conf/airframes/motor_bench.xml

@@ -14,7 +14,7 @@ LPC21ISP_XTAL = 12000
 #FLASH_MODE = IAP
 FLASH_MODE = ISP
 
-mb.CFLAGS += -DCONFIG=\"motor_bench.h\"
+mb.CFLAGS += -DCONFIG=\"motor_bench.h\" -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC(10e-3)'
 mb.srcs = main_motor_bench.c sys_time.c $(SRC_ARCH)/sys_time_hw.c $(SRC_ARCH)/armVIC.c
 
 mb.CFLAGS += -DLED
@@ -22,7 +22,7 @@ mb.CFLAGS += -DLED
 mb.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400
 mb.srcs += $(SRC_ARCH)/uart_hw.c
 
-mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart1 
+mb.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport -DDOWNLINK_DEVICE=Uart0 
 mb.srcs += downlink.c pprz_transport.c
 
 mb.CFLAGS += -DADC -DUSE_AD0 -DUSE_AD0_1 -DUSE_AD0_2 -DUSE_AD0_3 -DUSE_AD0_4

conf/messages.xml

@@ -363,6 +363,13 @@
       <field name="mode" type="uint8"/>
   </message>
 
+  <message name="MOTOR_BENCH_STATUS" ID="230">
+      <field name="time_ticks" type="uint32"/>
+      <field name="time_s" type="uint16"/>
+      <field name="throttle" type="uint16"/>
+      <field name="mode" type="uint8"/>
+  </message>
+
 </class>
 
 

sw/airborne/arm7/sys_time_hw.c

@@ -1,6 +1,9 @@
 #include "armVIC.h"
 #include "sys_time.h"
 
+
+uint32_t cpu_time_ticks;
+
 #if defined ACTUATORS
 #include ACTUATORS
 #endif /* ACTUATORS */

sw/airborne/arm7/sys_time_hw.h

@@ -35,6 +35,7 @@
 #include CONFIG
 #include "led.h"
 
+extern uint32_t cpu_time_ticks;
 static uint32_t last_periodic_event;
 
 void TIMER0_ISR ( void ) __attribute__((naked));
@@ -58,7 +59,8 @@ static inline void sys_time_init( void ) {
   /* enable timer 0                  */
   T0TCR = TCR_ENABLE;
 
-  cpu_time = 0;
+  cpu_time_sec = 0;
+  cpu_time_ticks = 0;
 
   /* select TIMER0 as IRQ    */
   VICIntSelect &= ~VIC_BIT(VIC_TIMER0);
@@ -78,15 +80,25 @@ static inline void sys_time_init( void ) {
 
 #define FIFTY_MS          SYS_TICS_OF_SEC( 50e-3 )
 #define AVR_PERIOD_MS     SYS_TICS_OF_SEC( 15.625e-3 )
+#ifndef PERIODIC_TASK_PERIOD
 #define PERIODIC_TASK_PERIOD AVR_PERIOD_MS
+#endif
+
+#define TIME_TICKS_PER_SEC SYS_TICS_OF_SEC(1)
 
 static inline bool_t sys_time_periodic( void ) {
   uint32_t now = T0TC;
-  if (now - last_periodic_event >= PERIODIC_TASK_PERIOD) {
+  uint32_t dif = now - last_periodic_event;
+  if ( dif >= PERIODIC_TASK_PERIOD) {
     last_periodic_event += PERIODIC_TASK_PERIOD;
+    cpu_time_ticks += PERIODIC_TASK_PERIOD;
+    if (cpu_time_ticks > TIME_TICKS_PER_SEC) {
+      cpu_time_ticks -= TIME_TICKS_PER_SEC;
+      cpu_time_sec++;
 #ifdef TIME_LED
       LED_TOGGLE(TIME_LED)
 #endif
+      }
     return TRUE;
   }
   return FALSE;

sw/airborne/main_motor_bench.c

@@ -16,14 +16,25 @@
 #include "messages.h"
 #include "downlink.h"
 
+#include "paparazzi.h"
+
 static inline void main_init( void );
 static inline void main_periodic_task( void );
 static inline void main_event_task( void);
 
+static inline void bench_periodic ( void );
+static inline bool_t prbs_10( void ) ;
+
+#define MODE_MANUAL 0
+#define MODE_RAMP   1
+#define MODE_STEP   2
+#define MODE_PRBS   3
+
+uint8_t mode;
+uint16_t throttle;
+
 int main( void ) {
   main_init();
-  LED_ON(1);
-  LED_ON(2);
   while (1) {
     if (sys_time_periodic())
       main_periodic_task();
@@ -40,28 +51,51 @@ static inline void main_init( void ) {
   adc_init();
   icp_scale_init();
   tacho_mb_init();
+  mode = MODE_PRBS;
   int_enable();
 }
 
-static uint32_t t0, t1;
-
 static inline void main_event_task( void ) {
 
 }
 
 static inline void main_periodic_task( void ) {
-  t0 = T0TC;
+  bench_periodic();
+  DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &throttle, &mode);
+
   static uint8_t cnt;
   cnt++;
   if (!(cnt%16)) {
-    //    LED_TOGGLE(1);
+    LED_TOGGLE(1);
-    uart0_transmit('#');
+    //  DOWNLINK_SEND_MOTOR_BENCH_STATUS(&cpu_time_ticks, &cpu_time_sec, &throttle, &mode);
-    Uart0PrintHex32(pulse_len);
-    uart0_transmit(' ');
-    Uart0PrintHex32(t_duration);
-    uart0_transmit('\n');
   }
 }
 
+#define PRBS_VAL1 (0.55*MAX_PPRZ)
+#define PRBS_VAL2 (0.65*MAX_PPRZ)
+
+static inline void bench_periodic ( void ) {
+  switch (mode) {
+  case MODE_MANUAL:
+    throttle = 0;
+    break;
+  case MODE_PRBS:
+    throttle = prbs_10() ? PRBS_VAL1 : PRBS_VAL2;
+    break;
+  }
+}
+
+static inline bool_t prbs_10( void ) {
+  static uint16_t lsr = 0xFFFF;
+  uint8_t feed =
+    bit_is_set(lsr,6) ^
+    bit_is_set(lsr,9);
+  uint8_t val = bit_is_set(lsr,9);
+  lsr = (lsr << 1) | feed;
+  return val;
+}
+
+
+
 
 

sw/airborne/sys_time.c

@@ -1,3 +1,3 @@
 #include "sys_time.h"
 
-uint16_t cpu_time;
+uint16_t cpu_time_sec;

sw/airborne/sys_time.h

@@ -4,7 +4,7 @@
 #include <inttypes.h>
 #include CONFIG
 
-extern uint16_t cpu_time;
+extern uint16_t cpu_time_sec;
 
 #include "sys_time_hw.h"
 

sw/logalizer/Makefile

@@ -38,6 +38,9 @@ CC = gcc
 CFLAGS=-g -O2 -Wall `pkg-config gtk+-2.0 --cflags`
 LDFLAGS=`pkg-config gtk+-2.0 --libs` -s -lgtkdatabox `pcre-config --libs` -lglibivy
 
+motor_bench : motor_bench.c
+	$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
+
 ahrsview : ahrsview.c sliding_plot.c
 	$(CC) $(CFLAGS) -g -o $@ $^ $(LDFLAGS)
 

sw/logalizer/motor_bench.c

@@ -0,0 +1,34 @@
+#include <glib.h>
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include <Ivy/ivy.h>
+#include <Ivy/ivyglibloop.h>
+
+#define TICK_PER_SEC 15000000.
+void on_MOTOR_BENCH_STATUS(IvyClientPtr app, void *user_data, int argc, char *argv[]){
+  guint time_tick = atoi(argv[0]);
+  guint time_sec = atoi(argv[1]);
+  guint throttle = atoi(argv[2]);
+  guint mode = atoi(argv[3]);
+
+  float time = (float)time_sec + (float)time_tick / TICK_PER_SEC;
+  printf("%f %d\n", time, throttle);
+
+}
+
+
+int main ( int argc, char** argv) {
+
+  //  g_timeout_add(16, timeout_callback, chan);
+
+  GMainLoop *ml =  g_main_loop_new(NULL, FALSE);
+  
+  IvyInit ("MotorBench", "MotorBench READY", NULL, NULL, NULL, NULL);
+  IvyBindMsg(on_MOTOR_BENCH_STATUS, NULL, "^\\S* MOTOR_BENCH_STATUS (\\S*) (\\S*) (\\S*) (\\S*)");
+  IvyStart("127.255.255.255");
+
+  g_main_loop_run(ml);
+  return 0;
+}