[paparazzi-commits] [4908] cube fixes

[Martin Mueller]

Revision: 4908
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4908
Author:   mmm
Date:     2010-05-11 14:26:35 +0000 (Tue, 11 May 2010)
Log Message:
-----------
cube fixes

Modified Paths:
--------------
    paparazzi3/trunk/conf/flight_plans/cube.xml
    paparazzi3/trunk/conf/settings/nav_cube.xml
    paparazzi3/trunk/sw/airborne/nav_cube.c

Modified: paparazzi3/trunk/conf/flight_plans/cube.xml
===================================================================
--- paparazzi3/trunk/conf/flight_plans/cube.xml 2010-05-11 10:46:55 UTC (rev 
4907)
+++ paparazzi3/trunk/conf/flight_plans/cube.xml 2010-05-11 14:26:35 UTC (rev 
4908)
@@ -7,9 +7,9 @@
 </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="HOME" x="-30.0" y="13.0"/>
+    <waypoint name="CENTER" x="10.0" y="1.0" alt="140.0"/>
+    <waypoint name="CR" x="-53.8" y="140.0"/>
 
     <waypoint name="_B" x="-200." y="200.0"/>
     <waypoint name="B0" x="-200." y="200.0"/>
@@ -42,13 +42,13 @@
       <set var="cube_nsect_z" value="2"/>
       <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="100"/>
-      <set var="cube_grid_x"  value="20"/>
-      <set var="cube_grid_z"  value="20"/>
+      <set var="cube_size_y"  value="50"/>
+      <set var="cube_size_z"  value="40"/>
+      <set var="cube_grid_x"  value="10"/>
+      <set var="cube_grid_z"  value="10"/>
       <set var="cube_offs_x"  value="0"/>
       <set var="cube_offs_y"  value="30"/>
-      <set var="cube_offs_z"  value="40"/>
+      <set var="cube_offs_z"  value="10"/>
       <deroute block="Fly lines in cubical"/>
     </block>
 

Modified: paparazzi3/trunk/conf/settings/nav_cube.xml
===================================================================
--- paparazzi3/trunk/conf/settings/nav_cube.xml 2010-05-11 10:46:55 UTC (rev 
4907)
+++ paparazzi3/trunk/conf/settings/nav_cube.xml 2010-05-11 14:26:35 UTC (rev 
4908)
@@ -7,9 +7,9 @@
       <dl_setting MIN="1" MAX="12"  STEP="1" VAR="cube_sect"    
module="nav_cube" handler="SetSect"/>
       <dl_setting MIN="1" MAX="50"  STEP="1" VAR="cube_grid_x"  
module="nav_cube" handler="SetGridX"/>
       <dl_setting MIN="1" MAX="50"  STEP="1" VAR="cube_grid_z"  
module="nav_cube" handler="SetGridZ"/>
-      <dl_setting MIN="1" MAX="200" STEP="1" VAR="cube_offs_x"  
module="nav_cube" handler="SetOffsX"/>
-      <dl_setting MIN="1" MAX="200" STEP="1" VAR="cube_offs_y"  
module="nav_cube" handler="SetOffsY"/>
-      <dl_setting MIN="1" MAX="200" STEP="1" VAR="cube_offs_z"  
module="nav_cube" handler="SetOffsZ"/>
+      <dl_setting MIN="-200" MAX="200" STEP="1" VAR="cube_offs_x"  
module="nav_cube" handler="SetOffsX"/>
+      <dl_setting MIN="-200" MAX="200" STEP="1" VAR="cube_offs_y"  
module="nav_cube" handler="SetOffsY"/>
+      <dl_setting MIN="-200" MAX="200" STEP="1" VAR="cube_offs_z"  
module="nav_cube" handler="SetOffsZ"/>
       <dl_setting MIN="1" MAX="500" STEP="1" VAR="cube_size_x"  
module="nav_cube" handler="SetSizeX"/>
       <dl_setting MIN="1" MAX="500" STEP="1" VAR="cube_size_y"  
module="nav_cube" handler="SetSizeY"/>
       <dl_setting MIN="1" MAX="500" STEP="1" VAR="cube_size_z"  
module="nav_cube" handler="SetSizeZ"/>

Modified: paparazzi3/trunk/sw/airborne/nav_cube.c
===================================================================
--- paparazzi3/trunk/sw/airborne/nav_cube.c     2010-05-11 10:46:55 UTC (rev 
4907)
+++ paparazzi3/trunk/sw/airborne/nav_cube.c     2010-05-11 14:26:35 UTC (rev 
4908)
@@ -73,7 +73,7 @@
   if (cube_nline_x >= MAX_LINES_X) cube_nline_x = MAX_LINES_X-1;
 
   /* position and number of layers in this sector */
-  cube_pos_z = (cube_sect-1) / cube_nsect_z;
+  cube_pos_z = (cube_sect-1) / cube_nsect_x;
   cube_line_z_start =  (cube_pos_z    * cube_nline_z_t)/cube_nsect_z;
   cube_line_z_end   = ((cube_pos_z+1) * cube_nline_z_t)/cube_nsect_z;
   cube_nline_z = cube_line_z_end - cube_line_z_start;
@@ -95,8 +95,8 @@
   start_by = start_ey - cube_size_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_bz = waypoints[center].a - (((cube_nline_z_t-1) * cube_grid_z)/2)
+             + (cube_line_z_start*cube_grid_z) + cube_offs_z;
   start_ez = start_bz;
 
   /* reset all waypoints to the standby position */
@@ -142,13 +142,13 @@
   waypoints[dest_b].y = waypoints[src_b+i].y;
   waypoints[dest_b].a = waypoints[src_b+i].a + j*cube_grid_z;
   /* always keep at least security altitude */
-  if (waypoints[dest_b].a < ground_alt+SECURITY_HEIGHT) waypoints[dest_b].a = 
ground_alt+SECURITY_HEIGHT;
+  if (waypoints[dest_b].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_b].a 
= ground_alt+SECURITY_HEIGHT;
 
   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;
   /* always keep at least security altitude */
-  if (waypoints[dest_e].a < ground_alt+SECURITY_HEIGHT) waypoints[dest_e].a = 
ground_alt+SECURITY_HEIGHT;
+  if (waypoints[dest_e].a < (ground_alt+SECURITY_HEIGHT)) waypoints[dest_e].a 
= ground_alt+SECURITY_HEIGHT;
 
   return FALSE; 
 }