conf/Makefile.stm32

@@ -33,6 +33,7 @@ MCU   = cortex-m3
 #DEBUG = dwarf-2
 #OPT   = s
 OPT   = 2
+#OPT   = 0
 
 # Programs location
 TOOLCHAIN_DIR=/opt/paparazzi/stm32

conf/airframes/Poine/booz2_a7.xml

@@ -163,18 +163,21 @@
  </section>
 
  
-  <firmware name="baloo">
+  <firmware name="rotorcraft">
     <target name="ap" board="lisa_l_1.0"/>
-    <subsystem name="radio_control" type="spektrum"/>
+    <subsystem name="radio_control" type="spektrum">
+      <param name="RADIO_CONTROL_SPEKTRUM_MODEL" 
+             value="\\\"booz/radio_control/booz_radio_control_spektrum_dx7se.h\\\""/>
+    </subsystem>
     <subsystem name="actuators"     type="asctec"/>
     <subsystem name="imu"           type="b2v1_1"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="ahrs"          type="cmpl"/>
   </firmware>
 
-  <firmware name="baloo">
+  <firmware name="rotorcraft">
-    <target name="test_led" board="lisa_l_1.0" />
+    <target name="test_led"      board="lisa_l_1.0" />
-    <target name="test_float" board="lisa_l_1.0" />
+    <target name="test_float"    board="lisa_l_1.0" />
     <target name="test_periodic" board="lisa_l_1.0" />
   </firmware>
 
@@ -194,7 +197,6 @@ SRC_FMS=fms
 
 
 ap.CFLAGS += -DMODEM_BAUD=B57600
-ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"radio_control/booz_radio_control_spektrum_dx7se.h\"
 
 include $(PAPARAZZI_SRC)/conf/autopilot/lisa_test_progs.makefile
 

conf/airframes/Poine/easy_glider1.xml

@@ -1,4 +1,4 @@
-<!DOCTYPE airframe SYSTEM "airframe.dtd">
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
 <airframe name="Easy Glider Tiny 0.99">
 

conf/airframes/UofAdelaide/lisa_a1000.xml

@@ -1,4 +1,4 @@
-<!-- this is an asctec frame equiped with  Lisa/L#3 and asctec V2 controllers -->
+<!-- this is a variable pitch quadrotor -->
 
 <airframe name="lisa_asctec">
 
@@ -168,17 +168,6 @@
  <makefile>
 
 
-
-USER = root
-HOST = 192.168.1.3
-#HOST = overo
-#HOST = beth
-#HOST = asctec_lisa
-TARGET_DIR = ~
-
-SRC_FMS=fms
-
-
 ARCH=stm32
 ARCHI=stm32
 BOARD_CFG=\"boards/lisa_0.99.h\"
@@ -202,11 +191,6 @@ include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
 
 include $(PAPARAZZI_SRC)/conf/autopilot/lisa_test_progs.makefile
 
-#
-# 
-#
-
-#include $(PAPARAZZI_SRC)/conf/autopilot/lisa_passthrough.makefile
 
  </makefile>
 

conf/autopilot/baloo.makefile

@@ -1,209 +0,0 @@
-#
-# $Id$
-#  
-# Copyright (C) 2010 The Paparazzi Team
-#
-# This file is part of Paparazzi.
-#
-# Paparazzi is free software; you can redistribute it and/or modify
-# it under the terms of the GNU General Public License as published by
-# the Free Software Foundation; either version 2, or (at your option)
-# any later version.
-#
-# Paparazzi is distributed in the hope that it will be useful,
-# but WITHOUT ANY WARRANTY; without even the implied warranty of
-# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-# GNU General Public License for more details.
-#
-# You should have received a copy of the GNU General Public License
-# along with Paparazzi; see the file COPYING.  If not, write to
-# the Free Software Foundation, 59 Temple Place - Suite 330,
-# Boston, MA 02111-1307, USA. 
-#
-#
-
-CFG_BALOO=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/baloo
-
-SRC_BOOZ=booz
-SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)
-SRC_BOOZ_TEST=$(SRC_BOOZ)/test
-
-SRC_BOOZ_PRIV=booz_priv
-
-CFG_BOOZ=$(PAPARAZZI_SRC)/conf/autopilot/
-
-BOOZ_INC = -I$(SRC_BOOZ) -I$(SRC_BOOZ_ARCH)
-
-
-ap.ARCHDIR = $(ARCHI)
-# this is supposedly ignored by the stm32 makefile
-ap.ARCH = arm7tdmi
-ap.TARGET = ap
-ap.TARGETDIR = ap
-
-
-ap.CFLAGS += $(BOOZ_INC)
-ap.CFLAGS += -DBOARD_CONFIG=$(BOARD_CFG) -DPERIPHERALS_AUTO_INIT
-ap.srcs    = $(SRC_BOOZ)/booz2_main.c
-
-ifeq ($(ARCHI), stm32) 
-ap.srcs += lisa/plug_sys.c
-endif
-#
-# Interrupts
-#
-ifeq ($(ARCHI), arm7)
-ap.srcs += $(SRC_ARCH)/armVIC.c
-else ifeq ($(ARCHI), stm32) 
-ap.srcs += $(SRC_ARCH)/stm32_exceptions.c
-ap.srcs += $(SRC_ARCH)/stm32_vector_table.c
-endif
-
-#
-# LEDs
-#
-ap.CFLAGS += -DUSE_LED
-ifeq ($(ARCHI), stm32) 
-ap.srcs += $(SRC_ARCH)/led_hw.c
-endif
-
-#
-# Systime
-#
-ap.CFLAGS += -DUSE_SYS_TIME
-ap.srcs += sys_time.c $(SRC_ARCH)/sys_time_hw.c
-ap.CFLAGS += -DPERIODIC_TASK_PERIOD='SYS_TICS_OF_SEC((1./512.))'
-ifeq ($(ARCHI), stm32) 
-ap.CFLAGS += -DSYS_TIME_LED=1
-endif
-
-#
-# Telemetry/Datalink
-#
-ap.srcs += $(SRC_ARCH)/uart_hw.c
-ap.CFLAGS += -DDOWNLINK -DDOWNLINK_TRANSPORT=PprzTransport
-ap.srcs += $(SRC_BOOZ)/booz2_telemetry.c \
-	   downlink.c \
-           pprz_transport.c
-ap.CFLAGS += -DDATALINK=PPRZ
-ap.srcs += $(SRC_BOOZ)/booz2_datalink.c
-
-ifeq ($(ARCHI), arm7)
-ap.CFLAGS += -DUSE_UART1  -DUART1_VIC_SLOT=6  -DUART1_BAUD=MODEM_BAUD
-ap.CFLAGS += -DDOWNLINK_DEVICE=Uart1 
-ap.CFLAGS += -DPPRZ_UART=Uart1
-else ifeq ($(ARCHI), stm32) 
-ap.CFLAGS += -DUSE_UART2 -DUART2_BAUD=MODEM_BAUD
-ap.CFLAGS += -DDOWNLINK_DEVICE=Uart2 
-ap.CFLAGS += -DPPRZ_UART=Uart2
-endif
-
-
-ap.srcs += $(SRC_BOOZ)/booz2_commands.c
-
-#
-# Radio control choice
-#
-# include booz2_radio_control_ppm.makefile
-# or
-# include booz2_radio_control_spektrum.makefile
-#
-
-#
-# Actuator choice
-#
-# include booz2_actuators_buss.makefile
-# or
-# include booz2_actuators_asctec.makefile
-#
-
-#
-# IMU choice
-#
-# include booz2_imu_b2v1.makefile
-# or
-# include booz2_imu_b2v1_1.makefile
-# or
-# include booz2_imu_crista.makefile
-#
-
-
-ifeq ($(ARCHI), arm7)
-ap.CFLAGS += -DBOOZ2_ANALOG_BARO_LED=2 -DBOOZ2_ANALOG_BARO_PERIOD='SYS_TICS_OF_SEC((1./100.))'
-ap.srcs += $(SRC_BOOZ)/booz2_analog_baro.c
-
-ap.CFLAGS += -DBOOZ2_ANALOG_BATTERY_PERIOD='SYS_TICS_OF_SEC((1./10.))'
-ap.srcs += $(SRC_BOOZ)/booz2_battery.c
-
-ap.CFLAGS += -DADC0_VIC_SLOT=2
-ap.CFLAGS += -DADC1_VIC_SLOT=3
-ap.srcs += $(SRC_BOOZ)/booz2_analog.c \
-           $(SRC_BOOZ_ARCH)/booz2_analog_hw.c
-else ifeq ($(ARCHI), stm32) 
-ap.srcs += lisa/lisa_analog_plug.c
-endif
-
-
-#
-# GPS choice
-# 
-# include booz2_gps.makefile
-# or
-# nothing
-#
-
-
-#
-# AHRS choice
-#
-# include booz2_ahrs_cmpl.makefile
-# or
-# include booz2_ahrs_lkf.makefile
-#
-
-ap.srcs += $(SRC_BOOZ)/booz2_autopilot.c
-
-ap.srcs += math/pprz_trig_int.c
-ap.srcs += $(SRC_BOOZ)/booz_stabilization.c
-ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_rate.c
-
-
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_TYPE_INT
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\"
-ap.CFLAGS += -DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_euler_int.h\"
-ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_ref_euler_int.c
-ap.srcs += $(SRC_BOOZ)/stabilization/booz_stabilization_attitude_euler_int.c
-
-ap.CFLAGS += -DUSE_NAVIGATION
-ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_h.c
-ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
-
-ap.srcs += $(SRC_BOOZ)/booz2_ins.c
-ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c math/pprz_geodetic_double.c
-
-#
-# INS choice
-#
-# include booz2_ins_hff.makefile
-# or
-# nothing
-#
-
-#  vertical filter float version
-ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
-ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)"
-
-ap.srcs += $(SRC_BOOZ)/booz2_navigation.c
-
-
-#
-# FMS  choice
-#
-# include booz2_fms_test_signal.makefile
-# or
-# include booz2_fms_datalink.makefile
-# or 
-# nothing
-#
-
-

conf/autopilot/rotorcraft.makefile

@@ -22,7 +22,7 @@
 #
 #
 
-CFG_BALOO=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/baloo
+CFG_ROTORCRAFT=$(PAPARAZZI_SRC)/conf/autopilot/subsystems/rotorcraft
 
 SRC_BOOZ=booz
 SRC_BOOZ_ARCH=$(SRC_BOOZ)/arch/$(ARCH)

conf/telemetry/telemetry_test_passthrough.xml

@@ -16,7 +16,8 @@
       <message name="IMU_GYRO"                period="0.05"/>
       <message name="IMU_ACCEL"               period="0.05"/>
       <message name="IMU_MAG"                 period="0.05"/>
-    </mode>
+      <message name="BOOZ_BARO2_RAW"          period="0.05"/>
+   </mode>
   </process>
 
 </telemetry>

sw/airborne/adc.h

@@ -1,22 +1,22 @@
 /*
  * Paparazzi adc functions
  *  
- * Copyright (C) 2003 Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2003-2010 Paparazzi team
  *
- * This file is part of paparazzi.
+ * This file is part of Paparazzi.
  *
- * paparazzi is free software; you can redistribute it and/or modify
+ * Paparazzi is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2, or (at your option)
  * any later version.
  *
- * paparazzi is distributed in the hope that it will be useful,
+ * Paparazzi is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
+ * along with Paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA. 
  *

sw/airborne/booz/arch/stm32/imu/booz_imu_aspirin_arch.h

@@ -10,6 +10,7 @@ extern void adxl345_write_to_reg(uint8_t addr, uint8_t val);
 extern void adxl345_clear_rx_buf(void);
 extern void adxl345_start_reading_data(void);
 
+#if 0
 #define OnI2CDone() {							\
     switch (imu_aspirin.status) {					\
     case AspirinStatusReadingGyro:					\
@@ -26,5 +27,6 @@ extern void adxl345_start_reading_data(void);
       break;								\
     }									\
   }
+#endif
 
 #endif /* BOOZ_IMU_ASPIRIN_ARCH_H */

sw/airborne/booz/booz2_debug.h

@@ -24,6 +24,10 @@
 #ifndef BOOZ2_DEBUG_H
 #define BOOZ2_DEBUG_H
 
+#define MY_ASSERT(cond) {			\
+    if (!(cond)) while(1);			\
+  }
+
 #ifdef BOOZ_DEBUG
 
 #include "std.h"

sw/airborne/booz/imu/booz_imu_aspirin.c

@@ -28,8 +28,8 @@ void booz_imu_impl_init(void) {
 void booz_imu_periodic(void) {
   if (imu_aspirin.status == AspirinStatusUninit) {
     configure_gyro();
-    configure_mag();
+    //    configure_mag();
-    //    configure_accel();
+    configure_accel();
     imu_aspirin.status = AspirinStatusIdle;
   }
   else
@@ -42,7 +42,12 @@ static void configure_gyro(void) {
   
   /* set gyro range to 2000deg/s and low pass at 256Hz */
   i2c2.buf[0] = ITG3200_REG_DLPF_FS;
-  i2c2.buf[1] = 0x03;
+  i2c2.buf[1] = (0x03<<3);
+  i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
+  while (!imu_aspirin.i2c_done);
+  /* set sample rate to 533Hz */
+  i2c2.buf[0] = ITG3200_REG_SMPLRT_DIV;
+  i2c2.buf[1] = 0x0E;
   i2c2_transmit(ITG3200_ADDR, 2, &imu_aspirin.i2c_done);
   while (!imu_aspirin.i2c_done);
   /* switch to gyroX clock */
@@ -93,4 +98,5 @@ static void configure_accel(void) {
   adxl345_clear_rx_buf();
   /* reads data once to bring interrupt line up */
   adxl345_start_reading_data();
+
 }

sw/airborne/booz/test/booz2_tunnel.c

@@ -28,7 +28,6 @@
 #include "sys_time.h"
 #include "led.h"
 #include "uart.h"
-#include "interrupt_hw.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );

sw/airborne/booz/test/booz_test_imu.c

@@ -131,7 +131,7 @@ static inline void on_gyro_accel_event(void) {
 
 
 static inline void on_mag_event(void) {
-  BoozImuScaleMag();
+  BoozImuScaleMag(booz_imu);
   static uint8_t cnt;
   cnt++;
   if (cnt > 1) cnt = 0;

sw/airborne/fms/fms_autopilot_msg.h

@@ -93,8 +93,8 @@ struct __attribute__ ((packed)) AutopilotMessagePTUp
   int16_t rc_aux3;
   int16_t rc_aux4;
   uint8_t rc_status;
-	float vane_angle1; 
+  float vane_angle1; 
-	float vane_angle2; 
+  float vane_angle2; 
   struct PTUpValidFlags valid;
   uint32_t stm_msg_cnt;
   uint32_t stm_crc_err_cnt;

sw/airborne/fms/overo_test_passthrough.c

@@ -114,6 +114,9 @@ static void dialog_with_io_proc() {
 
   otp.rc_status = in->rc_status;
 
+  otp.baro_abs  = in->pressure_absolute;
+  otp.baro_diff = in->pressure_differential;
+
 }
 
 

sw/airborne/fms/overo_test_passthrough.h

@@ -10,7 +10,9 @@ struct OveroTestPassthrough {
   uint32_t io_proc_err_cnt;
   uint16_t servos_outputs_usecs[6]; /* FIXME */
   uint8_t  rc_status;
-};
+  int16_t  baro_abs;
+  int16_t  baro_diff;
+  };
 
 extern struct OveroTestPassthrough otp;
 

sw/airborne/fms/overo_test_passthrough_telemetry.h

@@ -24,6 +24,8 @@
 #define PERIODIC_SEND_IMU_MAG(_transport)				\
   DOWNLINK_SEND_IMU_MAG(_transport, &otp.imu.mag.x, &otp.imu.mag.y, &otp.imu.mag.z)
 
+#define PERIODIC_SEND_BOOZ_BARO2_RAW(_transport)				\
+  DOWNLINK_SEND_BOOZ_BARO2_RAW(_transport, &otp.baro_abs, &otp.baro_diff)
 
 
 #endif /* OVERO_TEST_PASSTHROUGH_TELEMETRY_H */

sw/airborne/fms/overo_test_telemetry2.c

@@ -15,7 +15,7 @@
 #include "dl_protocol.h"
 #include "fms_network.h"
 
-#define GCS_HOST "10.10.13.32"
+#define GCS_HOST "10.31.4.7"
 #define GCS_PORT 4242
 #define DATALINK_PORT 4243
 

sw/airborne/fms/test_telemetry_2.c

@@ -11,7 +11,7 @@
 #include "udp_transport.h"
 #include "fms_network.h"
 
-#define GCS_HOST "192.168.1.100"
+#define GCS_HOST "10.31.4.7"
 #define GCS_PORT 4242
 
 #define TIMEOUT_DT_SEC  0
@@ -39,6 +39,8 @@ void timeout_cb(int fd, short event, void *arg) {
 
 int main(int argc, char** argv) {
 
+  printf("hello world\n");
+
   network = network_new(GCS_HOST, GCS_PORT);
 
   /* Initalize the event library */

sw/airborne/lisa/lisa_baro.c

@@ -39,6 +39,7 @@ void baro_periodic(void) {
   case LBS_INITIALIZING_DIFF:
     baro_set_current_register(BARO_DIFF_ADDR, 0x00);
     baro.status = LBS_INITIALIZING_DIFF_1;
+    //    baro.status = LBS_UNINITIALIZED;
     break;
   case LBS_INITIALIZING_DIFF_1:
   case LBS_READ_DIFF:

sw/airborne/lisa/lisa_stm_passthrough_main.c

@@ -179,24 +179,25 @@ static inline void on_vane_msg(void *data) {
 	new_vane = TRUE;
 	int zero = 0;
 	DOWNLINK_SEND_VANE_SENSOR(DefaultChannel, 
-														&(csc_vane_msg.vane_angle1), 
+				  &(csc_vane_msg.vane_angle1), 
-														&zero, 
+				  &zero, 
-														&zero, 
+				  &zero, 
-														&zero, 
+				  &zero, 
-														&zero, 
+				  &zero, 
-														&csc_vane_msg.vane_angle2, 
+				  &csc_vane_msg.vane_angle2, 
-														&zero, 
+				  &zero, 
-														&zero, 
+				  &zero, 
-														&zero, 
+				  &zero, 
-														&zero);
+				  &zero);
 }
 
 static inline void main_on_baro_diff(void) {
-	new_baro_diff = TRUE;
+  new_baro_diff = TRUE;
+  RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);});
 }
 
 static inline void main_on_baro_abs(void) {
-	new_baro_abs = TRUE;
+  new_baro_abs = TRUE;
 }
 
 static inline void main_event(void) {

sw/airborne/lisa/test/hs_gyro.c

@@ -96,7 +96,7 @@ static inline void on_gyro_accel_event(void) {
   cnt++;
   if (cnt > NB_SAMPLES) cnt = 0;
   samples[cnt] = booz_imu.MEASURED_SENSOR;
-  if (cnt == 19) {
+  if (cnt == NB_SAMPLES-1) {
     DOWNLINK_SEND_IMU_HS_GYRO(DefaultChannel, &axis, NB_SAMPLES, samples);
   }
 

sw/airborne/lisa/test/lisa_test_hmc5843.c

@@ -116,6 +116,8 @@ static inline void main_periodic_task( void ) {
     break;
   }
 
+  //  if (mag_state  == 4) mag_state=1;
+
   if (mag_state  < INITIALISZED) mag_state++;
 
 }
@@ -123,7 +125,7 @@ static inline void main_periodic_task( void ) {
 
 static inline void main_event_task( void ) {
 
-  if (mag_state == INITIALISZED && mag_ready_for_read) {
+  if (mag_state == INITIALISZED && mag_ready_for_read && i2c_done) {
     /* read mag */
     i2c2_receive(HMC5843_ADDR, 7, &i2c_done);
     reading_mag = TRUE;
@@ -195,5 +197,5 @@ void exti9_5_irq_handler(void) {
   if(EXTI_GetITStatus(EXTI_Line5) != RESET)
     EXTI_ClearITPendingBit(EXTI_Line5);
 
-  mag_ready_for_read = TRUE;
+  if (mag_state == INITIALISZED) mag_ready_for_read = TRUE;
 }

sw/airborne/lisa/test/lisa_test_itg3200.c

@@ -90,6 +90,11 @@ static inline void main_periodic_task( void ) {
     });
 
   switch (gyro_state) {
+
+  case 1:
+    i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
+    i2c2_transmit(ITG3200_ADDR,1, &i2c_done);
+    break;
   case 2:
     /* set gyro range to 2000deg/s and low pass at 256Hz */
     i2c2.buf[0] = ITG3200_REG_DLPF_FS;
@@ -123,6 +128,7 @@ static inline void main_periodic_task( void ) {
     break;
   }
 
+  //  if (gyro_state == 1) gyro_state = 0;
   if (gyro_state  < INITIALISZED) gyro_state++;
 
 }
@@ -134,7 +140,7 @@ static inline void main_periodic_task( void ) {
 
 static inline void main_event_task( void ) {
 
-  if (gyro_state == INITIALISZED && gyro_ready_for_read) {
+  if (gyro_state == INITIALISZED && gyro_ready_for_read && i2c_done) {
     /* reads 8 bytes from address 0x1b */
     i2c2.buf[0] = ITG3200_REG_TEMP_OUT_H;
     i2c2_transceive(ITG3200_ADDR,1, 8, &i2c_done);
@@ -236,7 +242,7 @@ void exti15_10_irq_handler(void) {
 
   //  DEBUG_S4_TOGGLE();
 
-  gyro_ready_for_read = TRUE;
+  if (gyro_state == INITIALISZED) gyro_ready_for_read = TRUE;
 
   //  DEBUG_S4_OFF();
 

sw/airborne/lisa/test/test_board.c

@@ -97,14 +97,16 @@ int main( void ) {
 }
 
 static inline void main_init( void ) {
+
   hw_init();
   sys_time_init();
   led_init();
   
   baro_init();
-  booz_actuators_pwm_hw_init();
+  booz_actuators_init();
   
-  cur_test = TestTypeNone;
+  //  cur_test = TestTypeNone;
+  cur_test = TestTypeBldc;
 
 }
 
@@ -151,7 +153,21 @@ static void test_none_event(void) {}
 static inline void test_baro_on_baro_diff(void);
 static inline void test_baro_on_baro_abs(void);
 static void test_baro_start(void) {all_led_green();}
-static void test_baro_periodic(void) { RunOnceEvery(2, {baro_periodic();}); }
+static void test_baro_periodic(void) { 
+  RunOnceEvery(2, {baro_periodic();});
+  RunOnceEvery(100,{
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, 
+			       &i2c2_errors.ack_fail_cnt,
+			       &i2c2_errors.miss_start_stop_cnt,
+			       &i2c2_errors.arb_lost_cnt,
+			       &i2c2_errors.over_under_cnt,
+			       &i2c2_errors.pec_recep_cnt,
+			       &i2c2_errors.timeout_tlow_cnt,
+			       &i2c2_errors.smbus_alert_cnt,
+			       &i2c2_errors.unexpected_event_cnt,
+			       &i2c2_errors.last_unexpected_event);
+    });
+}
 static void test_baro_event(void) {BaroEvent(test_baro_on_baro_abs, test_baro_on_baro_diff);}
 static inline void test_baro_on_baro_abs(void) {
   RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);});
@@ -160,16 +176,33 @@ static inline void test_baro_on_baro_diff(void) {
   RunOnceEvery(5,{DOWNLINK_SEND_BOOZ_BARO2_RAW(DefaultChannel, &baro.abs_raw, &baro.diff_raw);});
 }
 
+
 /*
  *
  * Test motor controller
  *
  */
+
 static void test_bldc_start(void) {}
 static void test_bldc_periodic(void) {
+
   i2c1_buf[0] = 0x04;
   i2c1_transmit(0x58, 1, NULL);
+
+  RunOnceEvery(100,{
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, 
+			       &i2c1_errors.ack_fail_cnt,
+			       &i2c1_errors.miss_start_stop_cnt,
+			       &i2c1_errors.arb_lost_cnt,
+			       &i2c1_errors.over_under_cnt,
+			       &i2c1_errors.pec_recep_cnt,
+			       &i2c1_errors.timeout_tlow_cnt,
+			       &i2c1_errors.smbus_alert_cnt,
+			       &i2c1_errors.unexpected_event_cnt,
+			       &i2c1_errors.last_unexpected_event);
+    });
 }
+
 static void test_bldc_event(void) {}
 
 

sw/airborne/lisa/test/test_bswap.c

@@ -1,6 +1,6 @@
 
 #include <inttypes.h>
-//#include <byteswap.h>
+#include "init_hw.h"
 
 
 #define MyByteSwap16(n)				     \
@@ -44,6 +44,9 @@ int main(void) {
   uint16_t foo = 12345;
   uint16_t bar = MyByteSwap16(foo);
   MyByteSwap16_1(foo,bar);
+  bar = __REV16(foo);
+
+
 
   uint32_t a = 23456;
   uint32_t b = MyByteSwap32(a);

sw/airborne/lisa/test_baro2.c

@@ -35,6 +35,7 @@
 #include "downlink.h"
 
 #include "lisa/lisa_baro.h"
+//#include "my_debug_servo.h"
 
 static inline void main_init( void );
 static inline void main_periodic_task( void );
@@ -60,6 +61,11 @@ static inline void main_init( void ) {
   hw_init();
   sys_time_init();
   baro_init();
+
+  //  DEBUG_SERVO1_INIT();
+  //  DEBUG_SERVO2_INIT();
+
+
 }
 
 
@@ -69,18 +75,20 @@ static inline void main_periodic_task( void ) {
   RunOnceEvery(2, {baro_periodic();});
   LED_PERIODIC();
   RunOnceEvery(256, {DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);});
-  RunOnceEvery(256, {
+  RunOnceEvery(256, 
-   DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
+    {
-                            &i2c2_errors.ack_fail_cnt,
+      DOWNLINK_SEND_I2C_ERRORS(DefaultChannel, 
-                            &i2c2_errors.miss_start_stop_cnt,
+			       &i2c2_errors.ack_fail_cnt,
-                            &i2c2_errors.arb_lost_cnt,
+			       &i2c2_errors.miss_start_stop_cnt,
-                            &i2c2_errors.over_under_cnt,
+			       &i2c2_errors.arb_lost_cnt,
-                            &i2c2_errors.pec_recep_cnt,
+			       &i2c2_errors.over_under_cnt,
-                            &i2c2_errors.timeout_tlow_cnt,
+			       &i2c2_errors.pec_recep_cnt,
-                            &i2c2_errors.smbus_alert_cnt,
+			       &i2c2_errors.timeout_tlow_cnt,
-                            &i2c2_errors.unexpected_event_cnt,
+			       &i2c2_errors.smbus_alert_cnt,
-                            &i2c2_errors.last_unexpected_event);
+			       &i2c2_errors.unexpected_event_cnt,
+			       &i2c2_errors.last_unexpected_event);
     });
+    
 }
 
 

sw/airborne/lisa/test_float.c

@@ -55,8 +55,8 @@ static inline void main_periodic( void ) {
   d += 0.0025;
   double d1 = sin(d);
 
-  //  float i = sqrt(f);  // nok
+  float i = sqrt(f);  // ok
-  float i = powf(f1, f1); // nok
+  //float i = powf(f1, f1); // nok
   //float i = atan2(f, f); // ok
   RunOnceEvery(10, {DOWNLINK_SEND_TEST_FORMAT(DefaultChannel, &d1, &i);});
 

sw/airborne/lisa/test_mc.c

@@ -57,13 +57,13 @@ int main(void) {
 static inline void main_init( void ) {
   hw_init();
   sys_time_init();
-  //main_i2c_init();
+  main_i2c_init();
-   test_gpios();
+  test_gpios();
 }
 
 static inline void main_periodic_task( void ) {
 
-  //  main_test_send();
+  main_test_send();
   LED_PERIODIC();
 }
 
@@ -146,7 +146,7 @@ static inline void main_test_send( void ) {
   //  return;
   
   /* Snd data */
-  I2C_SendData(I2C1, 0x00);
+  I2C_SendData(I2C1, 0x06);
 
   /* Test on EV8 and clear it */
   while(!I2C_CheckEvent(I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED));

sw/airborne/lisa/test_mc2.c

@@ -53,15 +53,8 @@ static inline void main_init( void ) {
 
 static inline void main_periodic_task( void ) {
   
-  //  LED_ON(4);
+  i2c1_buf[0] = 0x04;
-  //  static uint16_t i = 1000;
+  i2c1_transmit(0x58, 1, &i2c_done);
-  //  if (i) i--;
-  //  else {
-  //    LED_OFF(4);
-    LED_ON(5);
-    i2c1_buf[0] = 0x04;
-    i2c1_transmit(0x58, 1, &i2c_done);
-    //  }
 
   LED_PERIODIC();
 

sw/airborne/lisa/test_periodic.c

@@ -48,9 +48,9 @@ static inline void main_init( void ) {
 }
 
 static inline void main_periodic( void ) {
-  //  LED_TOGGLE(1);
 
   LED_PERIODIC();
+  
 }
 
 

sw/airborne/main_ap.c

@@ -1,7 +1,7 @@
 /*
  * $Id$
  *  
- * Copyright (C) 2003-2006  Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2003-2010  Paparazzi team
  *
  * This file is part of paparazzi.
  *

sw/airborne/modules/ins/ins.h

@@ -85,17 +85,17 @@ void parse_ins_buffer( uint8_t );
 
 #endif /** !SITL */
 
-#define InsEventCheckAndHandle(handler) { \
+#define InsEventCheckAndHandle(handler) {			\
-  if (InsBuffer()) {				  \
+    if (InsBuffer()) {						\
-    ReadInsBuffer();				  \
+      ReadInsBuffer();						\
-  }						                \
+    }						                \
-  if (ins_msg_received) {			\
+    if (ins_msg_received) {					\
-    LED_TOGGLE(2); \
+      LED_TOGGLE(2);						\
-    parse_ins_msg();          \
+      parse_ins_msg();						\
-    handler;                \
+      handler;							\
-    ins_msg_received = FALSE; \
+      ins_msg_received = FALSE;					\
-  }						                \
+    }						                \
-}
+  }
 
 
 #endif /* INS_H */

sw/airborne/stm32/adc_hw.h

@@ -1,22 +1,22 @@
 /*
- * $Id: adc_hw.h 2638 2008-08-18 01:30:27Z poine $
+ * $Id$
  *  
- * Copyright (C) 2008  Antoine Drouin
+ * Copyright (C) 2010  Paparazzi team
  *
- * This file is part of paparazzi.
+ * This file is part of Paparazzi.
  *
- * paparazzi is free software; you can redistribute it and/or modify
+ * Paparazzi is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
  * the Free Software Foundation; either version 2, or (at your option)
  * any later version.
  *
- * paparazzi is distributed in the hope that it will be useful,
+ * Paparazzi is distributed in the hope that it will be useful,
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
  *
  * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
+ * along with Paparazzi; see the file COPYING.  If not, write to
  * the Free Software Foundation, 59 Temple Place - Suite 330,
  * Boston, MA 02111-1307, USA. 
  *
@@ -25,6 +25,20 @@
 #ifndef ADC_HW_H
 #define ADC_HW_H
 
+/*
+ * Architecture dependant ADC functions for STM32
+ * For now only hard coded for Lisa/L
+ *
+ * Logic   STM32  
+ * ADC1    PC3     ADC13 
+ * ADC2    PC5     ADC15
+ * ADC3    PB0     ADC8
+ * ADC4    PB1     ADC9
+ * ADC5    PB2     BOOT1
+ *         PA0     ADC0   bat monitor        
+ */
+
+
 #define AdcBank0(x) (x)
 #define AdcBank1(x) (x+NB_ADC)
 

sw/airborne/stm32/stm32_vector_table.c

@@ -308,9 +308,9 @@ void null_handler(void){
  * 
  */
 
-void assert_param(void);
+//void assert_param(void);
 
-void assert_param(void){
+//void assert_param(void){
-
+//
-}
+//}
 

sw/lib/ocaml/cserial.c

@@ -46,7 +46,7 @@ value c_init_serial(value device, value speed)
 
   int br = baudrates[Int_val(speed)];
 
-  int fd = open(String_val(device), O_RDWR);
+  int fd = open(String_val(device), O_RDWR|O_NONBLOCK);
  
   if (fd == -1) failwith("opening modem serial device : fd < 0");
   

sw/simulator/nps/nps_autopilot_booz.c

@@ -62,10 +62,10 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
 
   if (time < 8) { /* start with a little bit of hovering */
     int32_t init_cmd[4];
-    init_cmd[COMMAND_THRUST] = 0.2493*SUPERVISION_MAX_MOTOR;
+    init_cmd[COMMAND_THRUST] = 0.253*SUPERVISION_MAX_MOTOR;
-    init_cmd[COMMAND_ROLL]  = 0;
+    init_cmd[COMMAND_ROLL]   = 0;
-    init_cmd[COMMAND_PITCH] = 0;
+    init_cmd[COMMAND_PITCH]  = 0;
-    init_cmd[COMMAND_YAW]   = 0;
+    init_cmd[COMMAND_YAW]    = 0;
     supervision_run(TRUE, FALSE, init_cmd);
   }
   for (uint8_t i=0; i<ACTUATORS_MKK_NB; i++)

sw/simulator/scilab/q1d/q1d_ins.sci

@@ -18,8 +18,14 @@ function [Xi1, Pi1] = ins_run(Xi, Pi, sensors_i, sensors_i1, dt)
         0  0   1     ];
   B = [ dt^2/2 dt 0]';
   
-  Qz  = 0.01*dt;
+  Qz  = 0.01*dt^2/2;
-  Qzd = 0.01 * dt^2/2;
+  Qzd = 0.01*dt;
+  
+  // FIXME: Qz and Qzd noise mismatch with dt
+  //Qz  = 0.01*dt;
+  //Qzd = 0.01*dt^2/2;
+
+  
   Qbias = 0.0001 * dt;
   Q = [ Qz  0    0
          0  Qzd  0

sw/simulator/scilab/q1d/q1d_run.sce

@@ -61,7 +61,7 @@ if 0
 end
 if 1
   k = find(time >= 5);
-  fdm_param(k) = 0.5*fdm_mass;
+  fdm_param(k) = 0.75*fdm_mass;
 end
 
 
@@ -213,3 +213,83 @@ ctl_display_ref(ctl_sp, ctl_ref_state, time);
 drawnow();
 
 end
+
+
+//
+// big filter play
+//
+exec("q1d_big_filter.sci");
+bfl_init(time);
+for i = 2:length(time)
+   bfl_predict(i,  ctl_command(i-1), dt);
+   if modulo(i,5) == 0
+     bfl_update_baro(i, sensors_state(SENSORS_BARO,i));
+   end
+   bfl_update_accel(i, sensors_state(SENSORS_ACCEL,i), ctl_command(i-1));
+end
+
+set("current_figure",5);
+clf();
+f=get("current_figure");
+f.figure_name="Big Filter";
+drawlater();
+
+  subplot(4, 2, 1);
+  plot2d(time, sensors_state(SENSORS_BARO,:),3);
+  plot2d(time, bflt_state(BF_Z, :), 5);
+  plot2d(time, fdm_state(FDM_Z,:),2);
+  legends(["Estimation", "Truth", "Measurement"],[5 2 3], with_box=%f, opt="ur");  
+  xtitle('Z');
+
+  subplot(4, 2, 3);
+  plot2d(time, bflt_state(BF_ZD, :),5);
+  plot2d(time, fdm_state(FDM_ZD,:),2);
+  legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur");
+  xtitle('ZD');
+
+  subplot(4, 2, 5);
+  plot2d(time, sensors_state(SENSORS_ACCEL_BIAS,:),2);
+  plot2d(time, bflt_state(BF_BIAS, :), 5);
+  legends(["Estimation", "Truth"],[5 2], with_box=%f, opt="ur"); 
+  xtitle('BIAS');
+
+  subplot(4, 2, 7);
+  plot2d(time, bflt_state(BF_C, :));
+  xtitle('C');
+
+  
+  subplot(4, 2, 2);
+  foo=[];
+  for i=1:length(time)
+    foo = [foo bflt_cov(BF_Z, BF_Z, i)];
+  end
+  plot2d(time, foo);
+  xtitle('COV ZZ');
+  
+
+  subplot(4, 2, 4);
+  foo=[];
+  for i=1:length(time)
+    foo = [foo bflt_cov(BF_ZD, BF_ZD, i)];
+  end
+  plot2d(time, foo);
+  xtitle('COV ZDZD');
+
+  subplot(4, 2, 6);
+  foo=[];
+  for i=1:length(time)
+    foo = [foo bflt_cov(BF_BIAS, BF_BIAS, i)];
+  end
+  plot2d(time, foo);
+  xtitle('COV BIASBIAS');
+
+  subplot(4, 2, 8);
+  foo=[];
+  for i=1:length(time)
+    foo = [foo bflt_cov(BF_C, BF_C, i)];
+  end
+  plot2d(time, foo);
+  xtitle('COV CC');
+  
+  drawnow();
+

sw/simulator/scilab/q3d/test_adapt.sce

@@ -17,7 +17,7 @@ exec('q3d_display.sci');
 
 t0 = 0;
 t1 = 4.;
-t2 = 8.;
+t2 = 10.;
 dt = 1/512;
 time1 = t0:dt:t1;
 time2 = t1:dt:t2;
@@ -48,11 +48,12 @@ if 0
 end
 
 ctl_init(time);
-adp_init(time, [19.5 157]', []);
+adp_init(time, [16.5 110]', []);
 sensors_init(time)
 
 for i=2:length(time)
   ctl_run(i-1, adp_est(1,i-1), adp_est(2,i-1)); 
+//  ctl_run(i-1, 16, 100); 
   fdm_run(i, ctl_motor_cmd(:,i-1));
   sensors_run(i);
   adp_run(i);

sw/simulator/scilab/q6d/povray/test.pov

@@ -0,0 +1,12 @@
+
+
+#include "q6d.inc"
+
+
+object { Q6D(0,0,0,0,0,0)}
+
+
+object {  AXIS_POVRAY(0,-1000,0,0,0,0) }
+
+
+object {  AXIS_NED() }