diff --git a/conf/airframes/funjeteth1.xml b/conf/airframes/funjeteth1.xml index d0094d0072..419c998f33 100644 --- a/conf/airframes/funjeteth1.xml +++ b/conf/airframes/funjeteth1.xml @@ -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 diff --git a/conf/flight_plans/cube.xml b/conf/flight_plans/cube.xml new file mode 100644 index 0000000000..9fd2958cf3 --- /dev/null +++ b/conf/flight_plans/cube.xml @@ -0,0 +1,60 @@ + + + + +
+#include "nav_cube.h" +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/nav.c b/sw/airborne/nav.c index 78db1f7ba5..b272b9e4ec 100644 --- a/sw/airborne/nav.c +++ b/sw/airborne/nav.c @@ -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 ) { diff --git a/sw/airborne/nav.h b/sw/airborne/nav.h index 054d0868aa..d7443dbab9 100644 --- a/sw/airborne/nav.h +++ b/sw/airborne/nav.h @@ -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; diff --git a/sw/airborne/nav_cube.c b/sw/airborne/nav_cube.c new file mode 100644 index 0000000000..e48016bbb1 --- /dev/null +++ b/sw/airborne/nav_cube.c @@ -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 ? */ + 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; +} + diff --git a/sw/airborne/nav_cube.h b/sw/airborne/nav_cube.h new file mode 100644 index 0000000000..a61aafe658 --- /dev/null +++ b/sw/airborne/nav_cube.h @@ -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 */ + diff --git a/sw/logalizer/sd2log.ml b/sw/logalizer/sd2log.ml index 67612eaadc..b13e93758d 100644 --- a/sw/logalizer/sd2log.ml +++ b/sw/logalizer/sd2log.ml @@ -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