[paparazzi-commits] [4740] New flight figure 'cube', increase SD card timestamp resolution to 100us.

[Martin Mueller]

Revision: 4740
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4740
Author:   mmm
Date:     2010-03-29 06:14:48 +0000 (Mon, 29 Mar 2010)
Log Message:
-----------
New flight figure 'cube', increase SD card timestamp resolution to 100us.

Modified Paths:
--------------
    paparazzi3/trunk/conf/airframes/funjeteth1.xml
    paparazzi3/trunk/sw/airborne/nav.c
    paparazzi3/trunk/sw/airborne/nav.h
    paparazzi3/trunk/sw/logalizer/sd2log.ml

Added Paths:
-----------
    paparazzi3/trunk/conf/flight_plans/cube.xml
    paparazzi3/trunk/sw/airborne/nav_cube.c
    paparazzi3/trunk/sw/airborne/nav_cube.h

Modified: paparazzi3/trunk/conf/airframes/funjeteth1.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/funjeteth1.xml      2010-03-26 13:39:19 UTC 
(rev 4739)
+++ paparazzi3/trunk/conf/airframes/funjeteth1.xml      2010-03-29 06:14:48 UTC 
(rev 4740)
@@ -239,7 +239,7 @@
 ap.CFLAGS += -DNAV -DAGR_CLIMB -DLOITER_TRIM
 ap.srcs += nav.c fw_h_ctl.c fw_v_ctl.c
 
-ap.srcs += nav_line.c
+ap.srcs += nav_line.c nav_cube.c
 ap.srcs += nav_survey_rectangle.c
 
 
@@ -256,7 +256,7 @@
 # Config for SITL simulation
 include $(PAPARAZZI_SRC)/conf/autopilot/sitl.makefile
 sim.CFLAGS += -DBOARD_CONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN
-sim.srcs += nav_line.c nav_survey_rectangle.c
+sim.srcs += nav_line.c nav_survey_rectangle.c nav_cube.c
 
 sim.srcs += joystick.c
 sim.CFLAGS += -DUSE_JOYSTICK

Added: paparazzi3/trunk/conf/flight_plans/cube.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/cube.xml                         (rev 0)
+++ paparazzi3/trunk/conf/flight_plans/cube.xml 2010-03-29 06:14:48 UTC (rev 
4740)
@@ -0,0 +1,60 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan ALT="130" GROUND_ALT="68" LAT0="52.26483" LON0="9.99394" 
MAX_DIST_FROM_HOME="1500" NAME="Gross Lobke Cube" QFU="270" 
SECURITY_HEIGHT="60">
+
+<header>
+#include "nav_cube.h"
+</header>
+
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="-175.0"/>
+    <waypoint name="CENTER" x="0.1" y="0.1" alt="150.0"/>
+    <waypoint name="CR" x="-53.8" y="-175.1"/>
+
+    <waypoint name="_B" x="-200." y="200.0"/>
+    <waypoint name="B0" x="-200." y="200.0"/>
+    <waypoint name="B1" x="-200." y="200.0"/>
+    <waypoint name="B2" x="-200." y="200.0"/>
+    <waypoint name="B3" x="-200." y="200.0"/>
+    <waypoint name="B4" x="-200." y="200.0"/>
+    <waypoint name="B5" x="-200." y="200.0"/>
+    <waypoint name="B6" x="-200." y="200.0"/>
+    <waypoint name="B7" x="-200." y="200.0"/>
+    <waypoint name="_E" x="-200." y="200.0"/>
+    <waypoint name="E0" x="-200." y="200.0"/>
+    <waypoint name="E1" x="-200." y="200.0"/>
+    <waypoint name="E2" x="-200." y="200.0"/>
+    <waypoint name="E3" x="-200." y="200.0"/>
+    <waypoint name="E4" x="-200." y="200.0"/>
+    <waypoint name="E5" x="-200." y="200.0"/>
+    <waypoint name="E6" x="-200." y="200.0"/>
+    <waypoint name="E7" x="-200." y="200.0"/>
+  </waypoints>
+
+  <blocks>
+    <block name="wait" strip_button="wait">
+      <circle radius="-nav_radius" wp="CR"/>
+    </block>
+    <block name="Fly lines in cubical" strip_button="Cube">
+      <set var="cube_alpha"  value="60"/>
+      <set var="cube_size_x" value="70"/>
+      <set var="cube_size_y" value="150"/>
+      <set var="cube_size_z" value="50"/>
+      <set var="cube_grid_x" value="20"/>
+      <set var="cube_grid_z" value="20"/>
+      <set var="cube_offs_x" value="0"/>
+      <set var="cube_offs_y" value="30"/>
+      <set var="cube_offs_z" value="40"/>
+      <call fun="nav_cube_init(WP_CENTER, WP_B0, WP_E0)"/>
+      <for from="0" to="cube_nline_z" var="j">
+        <for from="0" to="cube_nline_x" var="i">
+          <call fun="nav_cube(_var_j, _var_i, WP__B, WP__E, WP_B0, WP_E0)"/>
+          <oval p1="_B" p2="_E" radius="-nav_radius" until="(oval_status == 
OC1)"/>
+          <go approaching_time="0" from="_B" hmode="route" wp="_E"/>
+        </for>
+      </for>
+      <deroute block="wait"/>
+    </block>
+  </blocks>
+</flight_plan>
+

Modified: paparazzi3/trunk/sw/airborne/nav.c
===================================================================
--- paparazzi3/trunk/sw/airborne/nav.c  2010-03-26 13:39:19 UTC (rev 4739)
+++ paparazzi3/trunk/sw/airborne/nav.c  2010-03-29 06:14:48 UTC (rev 4740)
@@ -43,6 +43,8 @@
 
 #define RCLost() bit_is_set(fbw_state->status, RADIO_REALLY_LOST)
 
+enum oval_status oval_status;
+
 float last_x, last_y;
 
 /** Index of last waypoint. Used only in "go" stage in "route" horiz mode */
@@ -570,9 +572,6 @@
     Initial state is the route along the desired segment (OC2).
 */
 
-enum oval_status { OR12, OC2, OR21, OC1 };
-
-static enum oval_status oval_status;
 uint8_t nav_oval_count;
 
 void nav_oval_init( void ) {

Modified: paparazzi3/trunk/sw/airborne/nav.h
===================================================================
--- paparazzi3/trunk/sw/airborne/nav.h  2010-03-26 13:39:19 UTC (rev 4739)
+++ paparazzi3/trunk/sw/airborne/nav.h  2010-03-29 06:14:48 UTC (rev 4740)
@@ -42,6 +42,8 @@
 #define Square(_x) ((_x)*(_x))
 #define DistanceSquare(p1_x, p1_y, p2_x, p2_y) 
(Square(p1_x-p2_x)+Square(p1_y-p2_y))
 
+enum oval_status { OR12, OC2, OR21, OC1 };
+
 extern float cur_pos_x;
 extern float cur_pos_y;
 extern float last_x, last_y;

Added: paparazzi3/trunk/sw/airborne/nav_cube.c
===================================================================
--- paparazzi3/trunk/sw/airborne/nav_cube.c                             (rev 0)
+++ paparazzi3/trunk/sw/airborne/nav_cube.c     2010-03-29 06:14:48 UTC (rev 
4740)
@@ -0,0 +1,113 @@
+/*
+ * $Id: nav_cube.c 3600 2009-07-01 20:05:12Z hecto $
+ *  
+ * Copyright (C) 2010  Martin Mueller
+ *
+ * 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 nav_cube.c
+ *  \brief Navigation in a cube towards a center
+ *
+ */
+
+#include "airframe.h"
+#include "nav_cube.h"
+#include "nav.h"
+
+int32_t cube_alpha;
+int32_t cube_size_x, cube_size_y, cube_size_z;
+int32_t cube_grid_x, cube_grid_z;
+int32_t cube_offs_x, cube_offs_y, cube_offs_z;
+int32_t cube_nline_x, cube_nline_z;
+
+bool_t nav_cube_init(uint8_t center, uint8_t tb, uint8_t te) {
+
+  int32_t i, start_bx, start_by, start_bz, start_ex, start_ey, start_ez;
+  int32_t bx, by, ex, ey;
+  float alpha, cos_alpha, sin_alpha;
+
+  /* sanity checks */
+  if (cube_grid_x == 0) cube_nline_x = 1;
+  else cube_nline_x = cube_size_x / cube_grid_x + 1;
+  if (cube_grid_z == 0) cube_nline_z = 1;
+  else cube_nline_z = cube_size_z / cube_grid_z + 1;
+
+  /* do not do more than pre-set number of lines */
+  if (cube_nline_x >= MAX_LINES_X) cube_nline_x = 0;
+
+  /* do the costly stuff only once */
+  alpha = ((360. - cube_alpha) / 360.) * 2 * M_PI;
+  cos_alpha = cos(alpha);
+  sin_alpha = sin(alpha);
+
+  /* calculate lower left start begin/end x coord */
+  start_bx = waypoints[center].x - (((cube_nline_x-1) * cube_grid_x)/2) + 
cube_offs_x;
+  start_ex = waypoints[center].x - (((cube_nline_x-1) * cube_grid_x)/2) + 
cube_offs_x;
+
+  /* calculate lower left start begin point y coord */
+  start_by = waypoints[center].y - cube_offs_y - cube_size_y;
+
+  /* calculate lower left start end point y coord */
+  start_ey = waypoints[center].y - cube_offs_y;
+
+  /* calculate lower left start begin/end z coord */
+  start_bz = waypoints[center].a - (((cube_nline_z-1) * cube_grid_z)/2) + 
cube_offs_z;
+  start_ez = waypoints[center].a - (((cube_nline_z-1) * cube_grid_z)/2) + 
cube_offs_z;
+
+  for (i=0; i < cube_nline_x; i++) {
+    /* set waypoints and vectorize in regard to center */
+    bx = (start_bx + i*cube_grid_x) - waypoints[center].x;
+    by = start_by - waypoints[center].y;
+    ex = (start_ex + i*cube_grid_x) - waypoints[center].x;
+    ey = start_ey - waypoints[center].y;
+    /* rotate clockwise with alpha and un-vectorize*/
+    waypoints[tb+i].x = bx * cos_alpha - by * sin_alpha + waypoints[center].x;
+    waypoints[tb+i].y = bx * sin_alpha + by * cos_alpha + waypoints[center].y;
+    waypoints[tb+i].a = start_bz;
+    waypoints[te+i].x = ex * cos_alpha - ey * sin_alpha + waypoints[center].x;
+    waypoints[te+i].y = ex * sin_alpha + ey * cos_alpha + waypoints[center].y;
+    waypoints[te+i].a = start_ez;
+  }
+
+  /* bug in <for from="" to=""> ? */
+  cube_nline_x--;
+  cube_nline_z--;
+
+  return FALSE; 
+}
+
+bool_t nav_cube(int8_t j, int8_t i,
+                uint8_t dest_b, uint8_t dest_e, 
+                uint8_t src_b, uint8_t src_e) {
+
+  if (i > cube_nline_x) return FALSE;
+  if (j > cube_nline_z) return FALSE;
+
+  waypoints[dest_b].x = waypoints[src_b+i].x;
+  waypoints[dest_b].y = waypoints[src_b+i].y;
+  waypoints[dest_b].a = waypoints[src_b+i].a + j*cube_grid_z;
+
+  waypoints[dest_e].x = waypoints[src_e+i].x;
+  waypoints[dest_e].y = waypoints[src_e+i].y;
+  waypoints[dest_e].a = waypoints[src_e+i].a + j*cube_grid_z;
+
+  return FALSE; 
+}
+

Added: paparazzi3/trunk/sw/airborne/nav_cube.h
===================================================================
--- paparazzi3/trunk/sw/airborne/nav_cube.h                             (rev 0)
+++ paparazzi3/trunk/sw/airborne/nav_cube.h     2010-03-29 06:14:48 UTC (rev 
4740)
@@ -0,0 +1,50 @@
+/*
+ * $Id: nav_cube.h 1936 2007-10-23 12:12:38Z hecto $
+ *  
+ * Copyright (C) 2010  Martin Mueller
+ *
+ * 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 NAV_CUBE_H
+#define NAV_CUBE_H
+
+#include "std.h"
+
+#define MAX_LINES_X 8
+
+extern bool_t nav_cube_init(uint8_t turb, uint8_t tb, uint8_t te);
+bool_t nav_cube(int8_t j, int8_t i,
+                uint8_t dest_b, uint8_t dest_e, 
+                uint8_t src_b, uint8_t src_e);
+
+extern int32_t cube_alpha;     /* angle of flight direction to north, 
clockwise */
+extern int32_t cube_size_x;    /* size of the cube x (perpendicular to flight 
dir) */
+extern int32_t cube_size_y;    /* size of the cube y (in flight dir) */
+extern int32_t cube_size_z;    /* height of the cube z */
+extern int32_t cube_grid_x;    /* grid distance x (horizontal) */
+extern int32_t cube_grid_z;    /* grid distance z (vertical) */
+extern int32_t cube_offs_x;    /* offset to center x (horizontal) */
+extern int32_t cube_offs_y;    /* offset to center y (in direction) */
+extern int32_t cube_offs_z;    /* offset to center z (vertical) */
+extern int32_t cube_nline_x;   /* number of lines x (horizontal) */
+extern int32_t cube_nline_z;   /* number of lines z (vertical) */
+
+#endif /* NAV_CUBE_H */
+

Modified: paparazzi3/trunk/sw/logalizer/sd2log.ml
===================================================================
--- paparazzi3/trunk/sw/logalizer/sd2log.ml     2010-03-26 13:39:19 UTC (rev 
4739)
+++ paparazzi3/trunk/sw/logalizer/sd2log.ml     2010-03-29 06:14:48 UTC (rev 
4740)
@@ -143,7 +143,7 @@
     else
       let msg_descr = message_of_id log_msg msg_id in
       let timestamp = Int32.to_float log_msg.Logpprz.timestamp /. 1e4 in
-      fprintf f_out "%.3f %d %s\n" timestamp ac_id (string_of_message log_msg 
msg_descr vs);
+      fprintf f_out "%.4f %d %s\n" timestamp ac_id (string_of_message log_msg 
msg_descr vs);
       
       (** Looking for a date from a GPS message and a md5 from an ALIVE *)
       if log_msg.Logpprz.source = 0 then