[paparazzi-commits] [5498] -Update main_coder for new CAN init and openocd makefile board config parameter

[Paul Cox]

Revision: 5498
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5498
Author:   paulcox
Date:     2010-08-20 10:01:45 +0000 (Fri, 20 Aug 2010)
Log Message:
-----------
-Update main_coder for new CAN init and openocd makefile board config parameter
-overo azimuth control with low pass filter of azimuth gyro

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/Poine/beth.xml
    paparazzi3/trunk/conf/settings/settings_beth.xml
    paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
    paparazzi3/trunk/sw/airborne/beth/main_coders.c
    paparazzi3/trunk/sw/airborne/beth/main_stm32.c
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
    paparazzi3/trunk/sw/airborne/beth/overo_estimator.h

Modified: paparazzi3/trunk/conf/airframes/Poine/beth.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/conf/airframes/Poine/beth.xml      2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -173,6 +173,7 @@
 main_coders.ARCHDIR = stm32
 main_coders.LDSCRIPT = $(SRC_ARCH)/stm32f103rb_flash.ld
 main_coders.OOCD_INTERFACE = olimex-arm-usb-ocd
+main_coders.OOCD_BOARD = olimex_stm32_h103
 #main_coders.CFLAGS += -DBOARD_CONFIG=\"boards/olimex_stm32-h103.h\"
 main_coders.CFLAGS += -DBOARD_CONFIG=\"boards/beth.h\"
 
@@ -196,7 +197,13 @@
 #main_coders.CFLAGS += -DUSE_I2C2
 #main_coders.srcs += $(SRC_ARCH)/i2c_hw.c
 
-main_coders.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ -DBLINKENLIGHTS
+main_coders.CFLAGS += -DUSE_CAN1 -DUSE_USB_LP_CAN1_RX0_IRQ  -DUSE_CAN_EXT_ID
+main_coders.CFLAGS += \
+       -DCAN_PRESCALER=11 \
+       -DCAN_SJW_TQ=CAN_SJW_1tq \
+       -DCAN_BS1_TQ=CAN_BS1_3tq \
+       -DCAN_BS2_TQ=CAN_BS2_5tq \
+       -DCAN_ERR_RESUME=DISABLE
 main_coders.srcs += can.c $(SRC_ARCH)/can_hw.c
 main_coders.srcs += $(SRC_BETH)/bench_sensors_can.c
 

Modified: paparazzi3/trunk/conf/settings/settings_beth.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/conf/settings/settings_beth.xml    2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -9,15 +9,21 @@
       <dl_setting var="controller.azimuth_sp" min="-15" step="0.5" max="15" 
module="beth/overo_controller"  shortname="azim_sp" unit="rad" alt_unit="deg" 
alt_unit_coef="57.29578">
       </dl_setting>
 
-      <dl_setting var="controller.azim_gain" min="0.1" step=".05" max="2" 
module="beth/overo_estimator"  shortname="azim_gain">
+      <dl_setting var="controller.azim_gain" min="0" step=".01" max=".1" 
module="beth/overo_estimator"  shortname="azim_gain">
       </dl_setting>
 
+      <dl_setting var="controller.omega_tilt_ref" min="200" step="100" 
max="1200" module="beth/overo_controller"  shortname="tilt_omega_ref" 
unit="rad/s" alt_unit="deg/s" alt_unit_coef="57.29578">
+      </dl_setting>
+
       <dl_setting var="estimator.elevation_lp_coeff" min="0.01" step="0.01" 
max="1" module="beth/overo_estimator"  shortname="elev_lp_coeff">
       </dl_setting>
 
       <dl_setting var="estimator.tilt_lp_coeff" min="0.01" step="0.01" max="1" 
module="beth/overo_estimator"  shortname="tilt_lp_coeff">
       </dl_setting>
 
+      <dl_setting var="estimator.azimuth_lp_coeff" min="0.01" step="0.01" 
max="1" module="beth/overo_estimator"  shortname="azim_lp_coeff">
+      </dl_setting>
+
       <dl_setting var="controller.armed" min="0" step="1" max="2" shortname 
="mode"/>
 
     </dl_settings>

Modified: paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-20 
09:02:26 UTC (rev 5497)
+++ paparazzi3/trunk/sw/airborne/beth/bench_sensors_can.c       2010-08-20 
10:01:45 UTC (rev 5498)
@@ -45,10 +45,12 @@
     tempangle = (buf[3]<<8) | buf[2];
     if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x20;} else 
{bench_sensors.angle_3 = tempangle;}
     rx_bd2 = 0;
+    LED_TOGGLE(4);
   } 
   else {
     tempangle = (buf[3]<<8) | buf[2];
     if ((tempangle == 0 )||(tempangle > 6000)) {can_err_flags = 0x10;} else 
{bench_sensors.angle_1 = tempangle;}
     rx_bd1 = 0;
+    //LED_TOGGLE(5);
   }
 }

Modified: paparazzi3/trunk/sw/airborne/beth/main_coders.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/sw/airborne/beth/main_coders.c     2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -34,7 +34,7 @@
 static inline void main_event( void );
 
 static inline void main_init_adc(void);
-static inline void main_on_bench_sensors( void );
+//static inline void main_on_bench_sensors( void );
 
 //static inline void main_init_i2c2(void);
 //void i2c2_ev_irq_handler(void);
@@ -76,40 +76,38 @@
   hw_init();
   sys_time_init();
   main_init_adc();
-  //main_init_i2c2();
-  //i2c2_init();
-  can_init();
+  bench_sensors_init();
   int_enable();
- 
 }
 
 static inline void main_periodic( void ) {
 
-  RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
+  /*RunOnceEvery(10, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});*/
   
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, 
&coder_values[0], &coder_values[1]);});
   //RunOnceEvery(5, {DOWNLINK_SEND_ADC_GENERIC(DefaultChannel, &can1_status, 
&can1_pending);});
 
-  RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
-    &bench_sensors.angle_2,&bench_sensors.angle_3, &bench_sensors.current);});
+  /*RunOnceEvery(5, {DOWNLINK_SEND_BETH(DefaultChannel, &bench_sensors.angle_1,
+    &bench_sensors.angle_2,&bench_sensors.angle_3, 
&bench_sensors.current);});*/
   
   servos[0]=coder_values[0];
   servos[1]=coder_values[1];
   //use id=1 for azimuth board
-  can_transmit(1, 0, (uint8_t *)servos, 8);
-
+  can_transmit(1, (uint8_t *)servos, 8);
+  LED_TOGGLE(5);
 }
 
 
 static inline void main_event( void ) {
-  BenchSensorsEvent(main_on_bench_sensors);
+  //BenchSensorsEvent(main_on_bench_sensors);
 }
 
 
-static inline void main_on_bench_sensors( void ) {
+/*static inline void main_on_bench_sensors( void ) {
  
-}
+}*/
 
+
 #if 0
 /*
  *

Modified: paparazzi3/trunk/sw/airborne/beth/main_stm32.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/sw/airborne/beth/main_stm32.c      2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -98,9 +98,6 @@
   overo_link.up.msg.thrust_out = thrust_out;
   overo_link.up.msg.pitch_out = pitch_out;
 
-  booz2_commands[COMMAND_PITCH] = pitch_out;
-  booz2_commands[COMMAND_THRUST] = thrust_out;
-
   //stop the motors if we've lost SPI or CAN link
   //If SPI has had CRC error we don't stop motors
   if ((spi_msg_cnt == 0) || (can_err_flags != 0)) {
@@ -110,6 +107,8 @@
     overo_link.up.msg.can_errs = can_err_flags;
     overo_link.up.msg.pitch_out = PITCH_MAGIC_NUMBER;
   } else {
+    booz2_commands[COMMAND_PITCH] = pitch_out;
+    booz2_commands[COMMAND_THRUST] = thrust_out;
     actuators_set(TRUE);
   }
 }

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.c
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.c 2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -6,8 +6,9 @@
 struct OveroEstimator estimator;
 
 void estimator_init(void) {
-  estimator.tilt_lp_coeff = 0.9;
-  estimator.elevation_lp_coeff = 0.9;
+  estimator.tilt_lp_coeff = 0.5;
+  estimator.elevation_lp_coeff = 0.5;
+  estimator.azimuth_lp_coeff = 0.5;
 }
 
 //bench sensors z,y,x values passed in
@@ -23,10 +24,10 @@
 
   estimator.tilt = -(tilt_neutral - (int32_t)tilt_measure ) * tilt_scale;
   Bound(estimator.tilt,-89,89);
-  //low pass filter gyro values
+  //low pass filter tilt gyro
   estimator.tilt_dot = estimator.tilt_dot + 
                          estimator.tilt_lp_coeff * 
(RATE_FLOAT_OF_BFP(booz_imu.gyro.q) - estimator.tilt_dot);
-  /* Second order filter to be tested
+  /* Second order filter yet to be tested
   estimator.tilt_dot = estimator.tilt_dot * (2 - estimator.tilt_lp_coeff1 - 
estimator.tilt_lp_coeff2) +
                          estimator.tilt_lp_coeff1 * estimator.tilt_lp_coeff2 * 
RATE_FLOAT_OF_BFP(booz_imu.gyro.q) -
                          estimator.tilt_dot_old * (1 - 
estimator.tilt_lp_coeff1 - estimator.tilt_lp_coeff2 +
@@ -44,7 +45,11 @@
                               estimator.elevation_lp_coeff * (rotated_elev_dot 
- estimator.elevation_dot);
 
   estimator.azimuth = (azimuth_neutral - (int32_t)azimuth_measure ) * 
azimuth_scale;
-  estimator.azimuth_dot = RATE_FLOAT_OF_BFP(booz_imu.gyro.r);
 
+  //low pass filter azimuth gyro
+  //TODO: compensate rotation and increase order
+  estimator.azimuth_dot = estimator.azimuth_dot + 
+                         estimator.azimuth_lp_coeff * 
(RATE_FLOAT_OF_BFP(booz_imu.gyro.r) - estimator.azimuth_dot);
+
 }
 

Modified: paparazzi3/trunk/sw/airborne/beth/overo_estimator.h
===================================================================
--- paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-20 09:02:26 UTC 
(rev 5497)
+++ paparazzi3/trunk/sw/airborne/beth/overo_estimator.h 2010-08-20 10:01:45 UTC 
(rev 5498)
@@ -15,6 +15,7 @@
 
   float tilt_lp_coeff;
   float elevation_lp_coeff;
+  float azimuth_lp_coeff;
 };