Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 20:36:06 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

New flight figure 'cube', increase SD card timestamp resolution to 100us.

Browse Source
This commit is contained in:
Martin Mueller
2010-03-29 06:14:48 +00:00
parent 044f42e3ab
commit d48fac7199
7 changed files with 230 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/funjeteth1.xml
+2 -2
View File
@@ -239,7 +239,7 @@ ap.srcs += infrared.c estimator.c
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 @@ ap.CFLAGS += -DUSE_I2C0 -DUSE_BARO_SCP
# 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
conf/flight_plans/cube.xml
+60
View File
@@ -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>
sw/airborne/nav.c
+2 -3
View File
@@ -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 @@ void nav_eight(uint8_t target, uint8_t c1, float radius) {
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 ) {
sw/airborne/nav.h
+2
View File
@@ -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;
sw/airborne/nav_cube.c
+113
View File
@@ -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;
}
sw/airborne/nav_cube.h
+50
View File
@@ -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 */
sw/logalizer/sd2log.ml
+1 -1
View File
@@ -143,7 +143,7 @@ let convert_file = fun file ->
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