[paparazzi-commits] [4605] booz_cam is now a module

[Gautier Hattenberger]

Revision: 4605
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4605
Author:   gautier
Date:     2010-03-02 10:31:07 +0000 (Tue, 02 Mar 2010)
Log Message:
-----------
booz_cam is now a module

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/booz2_main.c
    paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/booz_cam.xml
    paparazzi3/trunk/conf/settings/booz_cam.xml
    paparazzi3/trunk/sw/airborne/modules/vision/
    paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.c
    paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.h
    paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c
    paparazzi3/trunk/sw/airborne/modules/vision/cam_track.h

Added: paparazzi3/trunk/conf/modules/booz_cam.xml
===================================================================
--- paparazzi3/trunk/conf/modules/booz_cam.xml                          (rev 0)
+++ paparazzi3/trunk/conf/modules/booz_cam.xml  2010-03-02 10:31:07 UTC (rev 
4605)
@@ -0,0 +1,15 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="vision">
+  <!-- depend require="booz_pwm|led" -->
+  <header>
+    <file name="booz_cam.h"/>
+  </header>
+  <init fun="booz_cam_init()"/>
+  <periodic fun="booz_cam_periodic()" freq="10."/>
+  <makefile>
+    <flag name="USE_CAM"/>
+    <file name="booz_cam.c"/>
+  </makefile>
+</module>
+

Copied: paparazzi3/trunk/conf/settings/booz_cam.xml (from rev 4603, 
paparazzi3/trunk/conf/settings/booz2_cam.xml)
===================================================================
--- paparazzi3/trunk/conf/settings/booz_cam.xml                         (rev 0)
+++ paparazzi3/trunk/conf/settings/booz_cam.xml 2010-03-02 10:31:07 UTC (rev 
4605)
@@ -0,0 +1,22 @@
+<settings>
+  <dl_settings>
+
+    <dl_settings NAME="CAM">
+      <dl_setting var="booz_cam_mode" MIN="0" STEP="1" MAX="3" 
module="booz_cam" shortname="mode" values="NONE|MAN|HEADING|WP" 
handler="SetCamMode">
+        <strip_button name="CN" value="0"/>
+        <strip_button name="CM" value="1"/>
+        <strip_button name="CH" value="2"/>
+        <strip_button name="CWP" value="3"/>
+        <key_press key="F1" value="0"/>
+        <key_press key="F2" value="1"/>
+      </dl_setting>
+      <dl_setting var="booz_cam_tilt_pwm" min="1000" step="1" max="2000" 
module="booz_cam" shortname="tilt_pwm"/>
+      <dl_setting var="booz_cam_tilt" min="-90" step="1" max="45" 
module="booz_cam" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
+        <strip_button name="Look Foreward" icon="lookfore.png" value="0"/>
+        <strip_button name="Look Down" icon="lookdown.png" value="-6434"/>
+      </dl_setting>
+      <dl_setting var="booz_cam_pan" MIN="0" STEP="1" MAX="360" 
module="booz_cam" shortname="pan" unit="1/2^12r" alt_unit="deg" 
alt_unit_coef="0.0139882"/>
+    </dl_settings>
+
+  </dl_settings>
+</settings>

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-03-02 09:13:17 UTC 
(rev 4604)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_main.c      2010-03-02 10:31:07 UTC 
(rev 4605)
@@ -53,10 +53,6 @@
 #include "booz_ahrs.h"
 #include "booz2_ins.h"
 
-#ifdef USE_CAM
-#include "booz2_cam.h"
-#endif
-
 #if defined USE_CAM || USE_DROP
 #include "booz2_pwm_hw.h"
 #endif
@@ -131,10 +127,6 @@
   booz2_gps_init();
 #endif
 
-#ifdef USE_CAM
-  booz2_cam_init();
-#endif
-
 #ifdef BOOZ2_SONAR
   booz2_sonar_init();
 #endif
@@ -200,10 +192,6 @@
   booz_gps_periodic();
 #endif
 
-#ifdef USE_CAM
-  RunOnceEvery(50,booz2_cam_periodic());
-#endif
-
 #ifdef BOOZ2_SONAR
   booz2_analog_periodic();
 #endif

Modified: paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-03-02 09:13:17 UTC 
(rev 4604)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_telemetry.h 2010-03-02 10:31:07 UTC 
(rev 4605)
@@ -616,6 +616,17 @@
                                   &booz2_guidance_h_command_body.psi); \
   }
 
+#define PERIODIC_SEND_BOOZ2_GUIDANCE_H_REF(_chan) { \
+  DOWNLINK_SEND_BOOZ2_GUIDANCE_H_REF_INT(_chan, \
+      &booz2_guidance_h_pos_sp.x, \
+      &booz2_guidance_h_pos_ref.x, \
+      &booz2_guidance_h_speed_ref.x, \
+      &booz2_guidance_h_accel_ref.x, \
+      &booz2_guidance_h_pos_sp.y, \
+      &booz2_guidance_h_pos_ref.y, \
+      &booz2_guidance_h_speed_ref.y, \
+      &booz2_guidance_h_accel_ref.y); \
+}
 
 #include "booz2_gps.h"
 #include "booz2_navigation.h"
@@ -695,8 +706,7 @@
   }
 
 #ifdef USE_CAM
-#include "booz2_cam.h"
-#define PERIODIC_SEND_BOOZ2_CAM(_chan) 
DOWNLINK_SEND_BOOZ2_CAM(_chan,&booz2_cam_tilt,&booz2_cam_pan);
+#define PERIODIC_SEND_BOOZ2_CAM(_chan) 
DOWNLINK_SEND_BOOZ2_CAM(_chan,&booz_cam_tilt,&booz_cam_pan);
 #else
 #define PERIODIC_SEND_BOOZ2_CAM(_chan) {}
 #endif
@@ -721,11 +731,26 @@
 
 #ifdef BOOZ2_SONAR
 #include "booz2_sonar.h"
-#define PERIODIC_SEND_BOOZ2_SONAR(_chan) 
DOWNLINK_SEND_BOOZ2_SONAR(_chan,&booz2_sonar_front,&booz2_sonar_back,&booz2_sonar_right,&booz2_sonar_left);
+#define PERIODIC_SEND_BOOZ2_SONAR(_chan) 
DOWNLINK_SEND_BOOZ2_SONAR(_chan,&booz2_sonar_1,&booz2_sonar_2,&booz2_sonar_3,&booz2_sonar_4);
 #else
 #define PERIODIC_SEND_BOOZ2_SONAR(_chan) {}
 #endif
 
+#ifdef BOOZ2_TRACK_CAM
+#include "cam_track.h"
+#define PERIODIC_SEND_CAM_TRACK(_chan) DOWNLINK_SEND_BOOZ_SIM_SPEED_POS(_chan, 
\
+    &target_accel_ned.x, \
+    &target_accel_ned.y, \
+    &target_accel_ned.z, \
+    &target_speed_ned.x, \
+    &target_speed_ned.y, \
+    &target_speed_ned.z, \
+    &target_pos_ned.x, \
+    &target_pos_ned.y, \
+    &target_pos_ned.z)
+#else
+#define PERIODIC_SEND_CAM_TRACK(_chan) {}
+#endif
 
 #include "settings.h"
 #define PERIODIC_SEND_DL_VALUE(_chan) PeriodicSendDlValue(_chan)

Added: paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.c                      
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.c      2010-03-02 
10:31:07 UTC (rev 4605)
@@ -0,0 +1,130 @@
+/* 
+ * $Id: $
+ *
+ * Copyright (C) 2009 Gautier Hattenberger <address@hidden>,
+ *                    Antoine Drouin <address@hidden>
+ *
+ * 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.  
+ */
+
+#include "booz_cam.h"
+#include "booz2_pwm_hw.h"
+#include "booz_ahrs.h"
+#include "booz2_navigation.h"
+#include "booz2_ins.h"
+#include "flight_plan.h"
+
+uint8_t booz_cam_mode;
+
+// Tilt definition
+#ifdef BOOZ_CAM_TILT_NEUTRAL
+int16_t booz_cam_tilt_pwm;
+int16_t booz_cam_tilt;
+#ifndef BOOZ_CAM_TILT_MIN
+#define BOOZ_CAM_TILT_MIN BOOZ_CAM_TILT_NEUTRAL
+#endif
+#ifndef BOOZ_CAM_TILT_MAX
+#define BOOZ_CAM_TILT_MAX BOOZ_CAM_TILT_NEUTRAL
+#endif
+#define BOOZ_CAM_USE_TILT 1
+#endif
+
+// Pan definition
+#ifdef BOOZ_CAM_PAN_NEUTRAL
+int16_t booz_cam_pan;
+#ifndef BOOZ_CAM_PAN_MIN
+#define BOOZ_CAM_PAN_MIN BOOZ_CAM_PAN_NEUTRAL
+#endif
+#ifndef BOOZ_CAM_PAN_MAX
+#define BOOZ_CAM_PAN_MAX BOOZ_CAM_PAN_NEUTRAL
+#endif
+#define BOOZ_CAM_USE_PAN 1
+#endif
+
+#if defined BOOZ_CAM_TILT_ANGLE_MIN && defined BOOZ_CAM_TILT_ANGLE_MAX && 
defined BOOZ_CAM_USE_TILT
+#define CAM_TA_MIN ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MIN)
+#define CAM_TA_MAX ANGLE_BFP_OF_REAL(BOOZ_CAM_TILT_ANGLE_MAX)
+#define BOOZ_CAM_USE_TILT_ANGLES 1
+#endif
+
+void booz_cam_init(void) {
+  booz_cam_mode = BOOZ_CAM_MODE_NONE;
+#ifdef BOOZ_CAM_USE_TILT
+  booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
+  Booz2SetPwmValue(booz_cam_tilt_pwm);
+  booz_cam_tilt = 0;
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+  booz_cam_pan = BOOZ_CAM_PAN_NEUTRAL;
+#endif
+  LED_ON(CAM_SWITCH_LED); // CAM OFF
+}
+
+void booz_cam_periodic(void) {
+
+  switch (booz_cam_mode) {
+    case BOOZ_CAM_MODE_NONE:
+#ifdef BOOZ_CAM_USE_TILT
+      booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+      booz_cam_pan = booz_ahrs.ltp_to_body_euler.psi;
+#endif
+      break;
+    case BOOZ_CAM_MODE_MANUAL:
+#ifdef BOOZ_CAM_USE_TILT
+      Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX);
+#endif
+      break;
+    case BOOZ_CAM_MODE_HEADING:
+#ifdef BOOZ_CAM_USE_TILT_ANGLES
+      Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
+      booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX - 
BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+      Bound(booz_cam_tilt_pwm,BOOZ_CAM_TILT_MIN,BOOZ_CAM_TILT_MAX);
+#endif
+#ifdef BOOZ_CAM_USE_PAN
+      Bound(booz_cam_pan,BOOZ_CAM_PAN_MIN,BOOZ_CAM_PAN_MAX);
+      nav_heading = booz_cam_pan;
+#endif
+      break;
+    case BOOZ_CAM_MODE_WP:
+#ifdef WP_CAM
+      {
+        struct Int32Vect2 diff;
+        VECT2_DIFF(diff, waypoints[WP_CAM], booz_ins_enu_pos);
+        INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
+        INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
+        nav_heading = booz_cam_pan;
+#ifdef BOOZ_CAM_USE_TILT_ANGLES
+        int32_t dist, height;
+        INT32_VECT2_NORM(dist, diff);
+        height = (waypoints[WP_CAM].z - booz_ins_enu_pos.z) >> INT32_POS_FRAC;
+        INT32_ATAN2(booz_cam_tilt, height, dist);
+        Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
+        booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + (BOOZ_CAM_TILT_MAX - 
BOOZ_CAM_TILT_MIN) * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
+        Bound(booz_cam_tilt_pwm, BOOZ_CAM_TILT_MIN, BOOZ_CAM_TILT_MAX);
+#endif
+      }
+#endif
+      break;
+  }
+#ifdef BOOZ_CAM_USE_TILT
+  Booz2SetPwmValue(booz_cam_tilt_pwm);
+#endif
+}
+

Added: paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.h                      
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/booz_cam.h      2010-03-02 
10:31:07 UTC (rev 4605)
@@ -0,0 +1,57 @@
+/* 
+ * $Id: $
+ *
+ * Copyright (C) 2009 Gautier Hattenberger <address@hidden>,
+ *                    Antoine Drouin <address@hidden>
+ *
+ * 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.  
+ */
+
+#ifndef BOOZ_CAM_H
+#define BOOZ_CAM_H
+
+#include "airframe.h"
+#include "std.h"
+#include "led.h"
+
+#define BOOZ_CAM_MODE_NONE     0
+#define BOOZ_CAM_MODE_MANUAL   1
+#define BOOZ_CAM_MODE_HEADING  2
+#define BOOZ_CAM_MODE_WP       3
+
+extern uint8_t booz_cam_mode;
+
+#ifdef BOOZ_CAM_TILT_NEUTRAL
+extern int16_t booz_cam_tilt_pwm;
+extern int16_t booz_cam_tilt;
+#endif
+#ifdef BOOZ_CAM_PAN_NEUTRAL
+extern int16_t booz_cam_pan;
+#endif
+
+extern void booz_cam_init(void);
+extern void booz_cam_periodic(void);
+
+#define booz_cam_SetCamMode(_v) { \
+  booz_cam_mode = _v; \
+  if (booz_cam_mode == BOOZ_CAM_MODE_NONE) { LED_ON(CAM_SWITCH_LED); } \
+  else { LED_OFF(CAM_SWITCH_LED); } \
+}
+
+#endif /* BOOZ2_CAM_H */
+

Added: paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c                     
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/cam_track.c     2010-03-02 
10:31:07 UTC (rev 4605)
@@ -0,0 +1,236 @@
+/*
+ * $Id: demo_module.c 3079 2009-03-11 16:55:42Z gautier $
+ *  
+ * Copyright (C) 2010  Gautier Hattenberger
+ *
+ * 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. 
+ *
+ */
+
+#include "cam_track.h"
+
+#include "booz2_ins.h"
+#include "booz_ahrs.h"
+
+#ifdef USE_HFF
+#include "ins/booz2_hf_float.h"
+#endif
+
+struct FloatVect3 target_pos_ned;
+struct FloatVect3 target_speed_ned;
+struct FloatVect3 target_accel_ned;
+
+struct FloatVect3 last_pos_ned;
+
+#define CAM_DATA_LEN  (3*4)
+#define CAM_START_1   0xFF
+#define CAM_START_2   0xFE
+#define CAM_END       0xF0
+
+#define UNINIT        0
+#define GOT_START_1   1
+#define GOT_START_2   2
+#define GOT_LEN       3
+#define GOT_DATA      4
+#define GOT_END       5
+
+#include "messages.h"
+#include "downlink.h"
+
+volatile uint8_t cam_msg_received;
+uint8_t cam_status;
+uint8_t cam_data_len;
+
+void track_init(void) {
+  booz_ins_ltp_initialised = TRUE; // ltp is initialized and centered on the 
target
+  booz_ins_update_on_agl = TRUE;   // use sonar to update agl (assume flat 
ground)
+
+  cam_status = UNINIT;
+  cam_data_len = CAM_DATA_LEN;
+
+}
+
+#include <stdio.h>
+void track_periodic_task(void) {
+  char cmd_msg[256];
+  uint8_t c = 0;
+
+  cmd_msg[c++] = 'A';
+  cmd_msg[c++] = ' ';
+  float phi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.phi);
+  if (phi > 0) cmd_msg[c++] = ' ';
+  else { cmd_msg[c++] = '-'; phi = -phi; }
+  cmd_msg[c++] = '0' + ((unsigned int) phi % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10*phi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (100*phi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (1000*phi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10000*phi) % 10);
+  cmd_msg[c++] = ' ';
+  float theta = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.theta);
+  if (theta > 0) cmd_msg[c++] = ' ';
+  else { cmd_msg[c++] = '-'; theta = -theta; }
+  cmd_msg[c++] = '0' + ((unsigned int) theta % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10*theta) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (100*theta) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (1000*theta) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10000*theta) % 10);
+  cmd_msg[c++] = ' ';
+  float psi = ANGLE_FLOAT_OF_BFP(booz_ahrs.ltp_to_body_euler.psi);
+  if (psi > 0) cmd_msg[c++] = ' ';
+  else { cmd_msg[c++] = '-'; psi = -psi; }
+  cmd_msg[c++] = '0' + ((unsigned int) psi % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10*psi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (100*psi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (1000*psi) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10000*psi) % 10);
+  cmd_msg[c++] = ' ';
+  float alt = -POS_FLOAT_OF_BFP(booz_ins_ltp_pos.z);
+  //alt = 0.40;
+  if (alt > 0) cmd_msg[c++] = ' ';
+  else { cmd_msg[c++] = '-'; alt = -alt; }
+  cmd_msg[c++] = '0' + ((unsigned int) (alt/10) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) alt % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (10*alt) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (100*alt) % 10);
+  cmd_msg[c++] = '0' + ((unsigned int) (1000*alt) % 10);
+  cmd_msg[c++] = ' ';
+  cmd_msg[c++] = '\n';;
+
+  int i;
+  for (i = 0; i < c; i++) {
+    CamUartSend1(cmd_msg[i]);
+  }
+  //DOWNLINK_SEND_DEBUG(DefaultChannel,c,cmd_msg);
+
+}
+
+void track_event(void) {
+  if (!booz_ins_ltp_initialised) {
+    booz_ins_ltp_initialised = TRUE;
+#ifdef USE_HFF
+    booz_ins_hff_realign = TRUE;
+#endif
+  }
+
+#ifdef USE_HFF
+  if (booz_ins_hff_realign) {
+    booz_ins_hff_realign = FALSE;
+    struct FloatVect2 pos, zero;
+    pos.x = -target_pos_ned.x;
+    pos.y = -target_pos_ned.y;
+    b2_hff_realign(pos, zero);
+  }
+  b2_hff_update_pos(-target_pos_ned.x, -target_pos_ned.y);
+  booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);
+  booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_state.ydotdot);
+  booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_state.xdot);
+  booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
+  booz_ins_ltp_pos.x   = POS_BFP_OF_REAL(b2_hff_state.x);
+  booz_ins_ltp_pos.y   = POS_BFP_OF_REAL(b2_hff_state.y);
+#else
+  // store pos in ins
+  booz_ins_ltp_pos.x = -(POS_BFP_OF_REAL(target_pos_ned.x));
+  booz_ins_ltp_pos.y = -(POS_BFP_OF_REAL(target_pos_ned.y));
+  // compute speed from last pos
+  // TODO get delta T
+  // store last pos
+  VECT3_COPY(last_pos_ned, target_pos_ned);
+#endif
+
+  b2_hff_lost_counter = 0;
+}
+
+#define CAM_MAX_PAYLOAD 254
+uint8_t cam_data_buf[CAM_MAX_PAYLOAD];
+uint8_t cam_data_idx;
+
+void parse_cam_msg( void ) {
+  uint8_t* ptr;
+  // pos x
+  ptr = (uint8_t*)(&(target_pos_ned.x));
+  *ptr = cam_data_buf[0];
+  ptr++;
+  *ptr = cam_data_buf[1];
+  ptr++;
+  *ptr = cam_data_buf[2];
+  ptr++;
+  *ptr = cam_data_buf[3];
+  // pos y
+  ptr = (uint8_t*)(&(target_pos_ned.y));
+  *ptr = cam_data_buf[4];
+  ptr++;
+  *ptr = cam_data_buf[5];
+  ptr++;
+  *ptr = cam_data_buf[6];
+  ptr++;
+  *ptr = cam_data_buf[7];
+  // pos z
+  ptr = (uint8_t*)(&(target_pos_ned.z));
+  *ptr = cam_data_buf[8];
+  ptr++;
+  *ptr = cam_data_buf[9];
+  ptr++;
+  *ptr = cam_data_buf[10];
+  ptr++;
+  *ptr = cam_data_buf[11];
+
+  //DOWNLINK_SEND_DEBUG(DefaultChannel,12,cam_data_buf);
+}
+
+void parse_cam_buffer( uint8_t c ) {
+  char bla[1];
+  bla[1] = c;
+  //DOWNLINK_SEND_DEBUG(DefaultChannel,1,bla);
+  switch (cam_status) {
+  case UNINIT:
+    if (c != CAM_START_1)
+      goto error;
+    cam_status++;
+    break;
+  case GOT_START_1:
+    if (c != CAM_START_2)
+      goto error;
+    cam_status++;
+    break;
+  case GOT_START_2:
+    cam_data_len = c;
+    if (cam_data_len > CAM_MAX_PAYLOAD)
+      goto error;
+    cam_data_idx = 0;
+    cam_status++;
+    break;
+  case GOT_LEN:
+    cam_data_buf[cam_data_idx] = c;
+    cam_data_idx++;
+    if (cam_data_idx >= cam_data_len)
+      cam_status++;
+    break;
+  case GOT_DATA:
+    if (c != CAM_END)
+      goto error;
+    cam_msg_received = TRUE;
+    goto restart;
+    break;
+  }
+  return;
+ error:  
+ restart:
+  cam_status = UNINIT;
+  return;
+}
+

Added: paparazzi3/trunk/sw/airborne/modules/vision/cam_track.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vision/cam_track.h                     
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/vision/cam_track.h     2010-03-02 
10:31:07 UTC (rev 4605)
@@ -0,0 +1,72 @@
+/*
+ * $Id: demo_module.h 3079 2009-03-11 16:55:42Z gautier $
+ *  
+ * Copyright (C) 2010  Gautier Hattenberger
+ *
+ * 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. 
+ *
+ */
+
+/** \file cam_track.h
+ * 
+ * blob tracking with cmucam
+ */
+
+#ifndef CAM_TRACK_H
+#define CAM_TRACK_H
+
+#include <inttypes.h>
+#include "math/pprz_algebra_float.h"
+
+extern struct FloatVect3 target_pos_ned;
+extern struct FloatVect3 target_speed_ned;
+extern struct FloatVect3 target_accel_ned;
+
+extern void track_init(void);
+extern void track_periodic_task(void);
+extern void track_event(void);
+
+extern volatile uint8_t cam_msg_received;
+extern void parse_cam_msg( void );
+extern void parse_cam_buffer( uint8_t );
+
+#include "uart.h"
+
+#define __CamLink(dev, _x) dev##_x
+#define _CamLink(dev, _x)  __CamLink(dev, _x)
+#define CamLink(_x) _CamLink(CAM_LINK, _x)
+
+#define CamBuffer() CamLink(ChAvailable())
+#define ReadCamBuffer() { while (CamLink(ChAvailable())&&!cam_msg_received) 
parse_cam_buffer(CamLink(Getch())); }
+#define CamUartSend1(c) CamLink(Transmit(c))
+#define CamUartInitParam(_a,_b,_c) CamLink(InitParam(_a,_b,_c))
+#define CamUartRunning CamLink(TxRunning)
+
+#define CamEventCheckAndHandle() { \
+  if (CamBuffer()) {                             \
+    ReadCamBuffer();                             \
+  }                                                            \
+  if (cam_msg_received) {                      \
+    parse_cam_msg();          \
+    track_event();            \
+    cam_msg_received = FALSE; \
+  }                                                            \
+}
+
+
+#endif