[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6020] move and rename booz2_navigation to rotorcraf
|
From: |
Felix Ruess |
|
Subject: |
[paparazzi-commits] [6020] move and rename booz2_navigation to rotorcraft navigation |
|
Date: |
Tue, 28 Sep 2010 19:00:55 +0000 |
Revision: 6020
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6020
Author: flixr
Date: 2010-09-28 19:00:55 +0000 (Tue, 28 Sep 2010)
Log Message:
-----------
move and rename booz2_navigation to rotorcraft navigation
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
paparazzi3/trunk/conf/messages.xml
paparazzi3/trunk/conf/settings/settings_booz2.xml
paparazzi3/trunk/conf/telemetry/booz_minimal.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml
paparazzi3/trunk/sw/logalizer/export.ml
Added Paths:
-----------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.h
Removed Paths:
-------------
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
Modified: paparazzi3/trunk/conf/autopilot/rotorcraft.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 16:56:07 UTC
(rev 6019)
+++ paparazzi3/trunk/conf/autopilot/rotorcraft.makefile 2010-09-28 19:00:55 UTC
(rev 6020)
@@ -210,7 +210,7 @@
ap.srcs += $(SRC_FIRMWARE)/ins/vf_float.c
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./512.)'
-ap.srcs += $(SRC_BOOZ)/booz2_navigation.c
+ap.srcs += $(SRC_FIRMWARE)/navigation.c
#
Modified: paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/conf/autopilot/subsystems/rotorcraft/fdm_nps.makefile
2010-09-28 19:00:55 UTC (rev 6020)
@@ -151,4 +151,4 @@
#
-sim.srcs += $(SRC_BOOZ)/booz2_navigation.c
+sim.srcs += $(SRC_FIRMWARE)/navigation.c
Modified: paparazzi3/trunk/conf/messages.xml
===================================================================
--- paparazzi3/trunk/conf/messages.xml 2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/conf/messages.xml 2010-09-28 19:00:55 UTC (rev 6020)
@@ -1134,7 +1134,7 @@
<field name="body_m22" type="int32" alt_unit=""
alt_unit_coef="0.0000610"/>
</message>
- <message name="BOOZ2_NAV_STATUS" id="159">
+ <message name="ROTORCRAFT_NAV_STATUS" id="159">
<field name="block_time" type="uint16" unit="s"/>
<field name="stage_time" type="uint16" unit="s"/>
<field name="cur_block" type="uint8"/>
Modified: paparazzi3/trunk/conf/settings/settings_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 16:56:07 UTC
(rev 6019)
+++ paparazzi3/trunk/conf/settings/settings_booz2.xml 2010-09-28 19:00:55 UTC
(rev 6020)
@@ -64,9 +64,9 @@
</dl_settings>
<dl_settings NAME="NAV">
- <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400"
module="booz2_navigation" unit="m" handler="SetFlightAltitude"/>
- <dl_setting var="nav_heading" MIN="0" STEP="1" MAX="360"
module="booz2_navigation" unit="1/2^12r" alt_unit="deg"
alt_unit_coef="0.0139882"/>
- <dl_setting var="nav_radius" MIN="-150" STEP="0.1" MAX="150"
module="booz2_navigation" unit="m"/>
+ <dl_setting var="flight_altitude" MIN="0" STEP="0.1" MAX="400"
module="navigation" unit="m" handler="SetFlightAltitude"/>
+ <dl_setting var="nav_heading" MIN="0" STEP="1" MAX="360"
module="navigation" unit="1/2^12r" alt_unit="deg" alt_unit_coef="0.0139882"/>
+ <dl_setting var="nav_radius" MIN="-150" STEP="0.1" MAX="150"
module="navigation" unit="m"/>
</dl_settings>
Modified: paparazzi3/trunk/conf/telemetry/booz_minimal.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/booz_minimal.xml 2010-09-28 16:56:07 UTC
(rev 6019)
+++ paparazzi3/trunk/conf/telemetry/booz_minimal.xml 2010-09-28 19:00:55 UTC
(rev 6020)
@@ -11,7 +11,7 @@
<message name="BOOZ2_FP" period="1.2"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="10.1"/>
- <message name="BOOZ2_NAV_STATUS" period="2.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="2.6"/>
<message name="WP_MOVED" period="6.3"/>
</mode>
@@ -21,7 +21,7 @@
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
- <message name="BOOZ2_NAV_STATUS" period="1.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
<message name="BOOZ2_GPS" period=".25"/>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 16:56:07 UTC
(rev 6019)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2.xml 2010-09-28 19:00:55 UTC
(rev 6020)
@@ -11,7 +11,7 @@
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
- <message name="BOOZ2_NAV_STATUS" period="1.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
<message name="BOOZ2_GPS" period=".25"/>
@@ -90,7 +90,7 @@
<!--<message name="BOOZ2_STAB_ATTITUDE_REF" period=".4"/>-->
<message name="BOOZ2_FP" period="0.8"/>
<message name="BOOZ_STATUS" period="1.2"/>
- <message name="BOOZ2_NAV_STATUS" period="1.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="HFF_GPS" period=".03"/>
<message name="INS_REF" period="5.1"/>
</mode>
Modified: paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml
===================================================================
--- paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/conf/telemetry/telemetry_booz2_flixr.xml 2010-09-28
19:00:55 UTC (rev 6020)
@@ -11,7 +11,7 @@
<message name="BOOZ2_FP" period="0.25"/>
<message name="ALIVE" period="2.1"/>
<message name="INS_REF" period="5.1"/>
- <message name="BOOZ2_NAV_STATUS" period="1.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_CAM" period="1."/>
<message name="BOOZ2_GPS" period=".25"/>
@@ -89,7 +89,7 @@
<!--<message name="BOOZ2_STAB_ATTITUDE_REF" period=".4"/>-->
<message name="BOOZ2_FP" period="0.8"/>
<message name="BOOZ_STATUS" period="1.2"/>
- <message name="BOOZ2_NAV_STATUS" period="1.6"/>
+ <message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
<message name="HFF_GPS" period=".1"/>
<message name="INS_REF" period="5.1"/>
</mode>
Modified: paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-09-28 16:56:07 UTC
(rev 6019)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_datalink.c 2010-09-28 19:00:55 UTC
(rev 6020)
@@ -38,7 +38,7 @@
#include "booz_fms.h"
#endif
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#include "math/pprz_geodetic_int.h"
#include <firmwares/rotorcraft/ins.h>
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c 2010-09-28
19:00:55 UTC (rev 6020)
@@ -1,344 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 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.
- */
-
-#define NAV_C
-
-#include "booz2_navigation.h"
-
-#include "booz/booz2_debug.h"
-#include "booz_gps.h"
-#include <firmwares/rotorcraft/ins.h>
-
-#include <firmwares/rotorcraft/autopilot.h>
-#include "modules.h"
-#include "flight_plan.h"
-
-#include "math/pprz_algebra_int.h"
-
-const uint8_t nb_waypoint = NB_WAYPOINT;
-struct EnuCoor_f waypoints_float[NB_WAYPOINT] = WAYPOINTS;
-struct EnuCoor_i waypoints[NB_WAYPOINT];
-
-struct EnuCoor_i booz2_navigation_target;
-struct EnuCoor_i booz2_navigation_carrot;
-
-struct EnuCoor_i nav_last_point;
-
-uint16_t stage_time, block_time;
-
-uint8_t nav_stage, nav_block;
-uint8_t last_block, last_stage;
-uint8_t last_wp __attribute__ ((unused));
-
-int32_t ground_alt;
-
-uint8_t horizontal_mode;
-uint8_t nav_segment_start, nav_segment_end;
-uint8_t nav_circle_centre;
-int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
-
-int32_t nav_roll, nav_pitch;
-int32_t nav_heading, nav_course;
-float nav_radius;
-
-#ifndef DEFAULT_CIRCLE_RADIUS
-#define DEFAULT_CIRCLE_RADIUS 0.
-#endif
-
-uint8_t vertical_mode;
-uint32_t nav_throttle;
-int32_t nav_climb, nav_altitude, nav_flight_altitude;
-float flight_altitude;
-
-static inline void nav_set_altitude( void );
-
-#define CLOSE_TO_WAYPOINT (15 << 8)
-#define ARRIVED_AT_WAYPOINT (3 << 8)
-#define CARROT_DIST (12 << 8)
-
-void booz2_nav_init(void) {
- // init int32 waypoints
- uint8_t i = 0;
- for (i = 0; i < nb_waypoint; i++) {
- waypoints[i].x = POS_BFP_OF_REAL(waypoints_float[i].x);
- waypoints[i].y = POS_BFP_OF_REAL(waypoints_float[i].y);
- waypoints[i].z = POS_BFP_OF_REAL((waypoints_float[i].z - GROUND_ALT));
- }
- nav_block = 0;
- nav_stage = 0;
- ground_alt = POS_BFP_OF_REAL(GROUND_ALT);
- nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
- nav_flight_altitude = nav_altitude;
- flight_altitude = SECURITY_ALT;
- INT32_VECT3_COPY( booz2_navigation_target, waypoints[WP_HOME]);
- INT32_VECT3_COPY( booz2_navigation_carrot, waypoints[WP_HOME]);
-
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
- vertical_mode = VERTICAL_MODE_ALT;
-
- nav_roll = 0;
- nav_pitch = 0;
- nav_heading = 0;
- nav_course = 0;
- nav_radius = DEFAULT_CIRCLE_RADIUS;
- nav_throttle = 0;
- nav_climb = 0;
-
-}
-
-void booz2_nav_run(void) {
-
- /* compute a vector to the waypoint */
- struct Int32Vect2 path_to_waypoint;
- VECT2_DIFF(path_to_waypoint, booz2_navigation_target, ins_enu_pos);
-
- /* saturate it */
- VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
-
- int32_t dist_to_waypoint;
- INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
-
-#ifndef GUIDANCE_H_USE_REF
- if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
- VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target);
- }
- else {
- struct Int32Vect2 path_to_carrot;
- VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
- VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
- VECT2_SUM(booz2_navigation_carrot, path_to_carrot, ins_enu_pos);
- }
-#else
- // if H_REF is used, CARROT_DIST is not used
- VECT2_COPY(booz2_navigation_carrot, booz2_navigation_target);
-#endif
-
- nav_set_altitude();
-}
-
-void nav_circle(uint8_t wp_center, int32_t radius) {
- if (radius == 0) {
- VECT2_COPY(booz2_navigation_target, waypoints[wp_center]);
- }
- else {
- struct Int32Vect2 pos_diff;
- VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]);
- // go back to half metric precision or values are too large
- //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
- // store last qdr
- int32_t last_qdr = nav_circle_qdr;
- // compute qdr
- INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
- // increment circle radians
- if (nav_circle_radians != 0) {
- int32_t angle_diff = nav_circle_qdr - last_qdr;
- INT32_ANGLE_NORMALIZE(angle_diff);
- nav_circle_radians += angle_diff;
- }
- else {
- // Smallest angle to increment at next step
- nav_circle_radians = 1;
- }
-
- // direction of rotation
- int8_t sign_radius = radius > 0 ? 1 : -1;
- // absolute radius
- int32_t abs_radius = abs(radius);
- // carrot_angle
- int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
- Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
- carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
- int32_t s_carrot, c_carrot;
- PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
- PPRZ_ITRIG_COS(c_carrot, carrot_angle);
- // compute setpoint
- VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
- INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
- VECT2_SUM(booz2_navigation_target, waypoints[wp_center], pos_diff);
- }
- nav_circle_centre = wp_center;
- nav_circle_radius = radius;
- horizontal_mode = HORIZONTAL_MODE_CIRCLE;
-}
-
-
-//#include "stdio.h"
-void nav_route(uint8_t wp_start, uint8_t wp_end) {
- struct Int32Vect2 wp_diff,pos_diff;
- VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
- VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]);
- // go back to metric precision or values are too large
- INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
- INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
- int32_t leg_length;
- int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
- INT32_SQRT(leg_length,leg_length2);
- int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y)
/ leg_length;
- int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
- nav_leg_progress += progress;
- int32_t prog_2 = leg_length;// + progress / 2;
- Bound(nav_leg_progress, 0, prog_2);
- struct Int32Vect2 progress_pos;
- VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
- VECT2_SDIV(progress_pos, progress_pos, leg_length);
- INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
- VECT2_SUM(booz2_navigation_target,waypoints[wp_start],progress_pos);
- //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
- // booz2_navigation_target.x,
- // booz2_navigation_target.y,
- // progress_pos.x,
- // progress_pos.y,
- // waypoints[wp_start].x,
- // waypoints[wp_start].y,
- // leg_length, leg_length2, nav_leg_progress);
- //fflush(stdout);
-
- nav_segment_start = wp_start;
- nav_segment_end = wp_end;
- horizontal_mode = HORIZONTAL_MODE_ROUTE;
-}
-
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
- int32_t dist_to_point;
- struct Int32Vect2 diff;
- static uint8_t time_at_wp = 0;
- VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos);
- INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
- INT32_VECT2_NORM(dist_to_point, diff);
- //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
- //fflush(stdout);
- //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
- if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
- else time_at_wp = 0;
- if (time_at_wp > 20) return TRUE;
- if (from_idx > 0 && from_idx < NB_WAYPOINT) {
- struct Int32Vect2 from_diff;
- VECT2_DIFF(from_diff, waypoints[wp_idx],waypoints[from_idx]);
- INT32_VECT2_RSHIFT(from_diff,from_diff,INT32_POS_FRAC);
- return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
- }
- else return FALSE;
-}
-
-static inline void nav_set_altitude( void ) {
- static int32_t last_nav_alt = 0;
- if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
- nav_flight_altitude = nav_altitude;
- last_nav_alt = nav_altitude;
- }
-}
-
-/** Reset the geographic reference to the current GPS fix */
-unit_t nav_reset_reference( void ) {
- ins_ltp_initialised = FALSE;
- ins_hf_realign = TRUE;
- ins_vf_realign = TRUE;
- return 0;
-}
-
-unit_t nav_reset_alt( void ) {
- ins_vf_realign = TRUE;
-
-#ifdef USE_GPS
- ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
- ins_ltp_def.hmsl = booz_gps_state.hmsl;
-#endif
-
- return 0;
-}
-
-void nav_init_stage( void ) {
- INT32_VECT3_COPY(nav_last_point, ins_enu_pos);
- stage_time = 0;
- nav_circle_radians = 0;
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
-}
-
-void nav_init_block(void) {
- if (nav_block >= NB_BLOCK)
- nav_block=NB_BLOCK-1;
- nav_stage = 0;
- block_time = 0;
- InitStage();
-}
-
-void nav_goto_block(uint8_t b) {
- if (b != nav_block) { /* To avoid a loop in a the current block */
- last_block = nav_block;
- last_stage = nav_stage;
- }
- GotoBlock(b);
-}
-
-#include <stdio.h>
-void nav_periodic_task() {
- RunOnceEvery(16, { stage_time++; block_time++; });
-
- /* from flight_plan.h */
- auto_nav();
-
- /* run carrot loop */
- booz2_nav_run();
-
- ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.);
-
-}
-
-#include "downlink.h"
-#include "messages.h"
-#include "uart.h"
-void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
- if (wp_id < nb_waypoint) {
- INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
- DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &(new_pos->x),
&(new_pos->y), &(new_pos->z));
- }
-}
-
-void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp,
int16_t heading_rate_sp ) {
- MY_ASSERT(wp < nb_waypoint);
- int32_t s_heading, c_heading;
- PPRZ_ITRIG_SIN(s_heading, nav_heading);
- PPRZ_ITRIG_COS(c_heading, nav_heading);
- // FIXME : scale POS to SPEED
- struct Int32Vect3 delta_pos;
- VECT3_SDIV(delta_pos, speed_sp,BOOZ2_NAV_FREQ); /* fixme :make sure the
division is really a >> */
- INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
- waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >>
INT32_TRIG_FRAC;
- waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >>
INT32_TRIG_FRAC;
- waypoints[wp].z += delta_pos.z;
- int32_t delta_heading = heading_rate_sp / BOOZ2_NAV_FREQ;
- delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
- nav_heading += delta_heading;
-
- INT32_COURSE_NORMALIZE(nav_heading);
- RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp,
&(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
-}
-
-bool_t nav_detect_ground(void) {
- if (!autopilot_detect_ground) return FALSE;
- autopilot_detect_ground = FALSE;
- return TRUE;
-}
-
-void nav_home(void) {}
-
Deleted: paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h 2010-09-28
19:00:55 UTC (rev 6020)
@@ -1,222 +0,0 @@
-/*
- * $Id$
- *
- * Copyright (C) 2008-2009 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 BOOZ2_NAVIGATION_H
-#define BOOZ2_NAVIGATION_H
-
-#include "std.h"
-#include "math/pprz_geodetic_int.h"
-#include "math/pprz_geodetic_float.h"
-
-#define BOOZ2_NAV_FREQ 16
-// FIXME use periodic FREQ
-#define BOOZ2_NAV_PRESCALER (512/BOOZ2_NAV_FREQ)
-
-extern struct EnuCoor_i booz2_navigation_target;
-extern struct EnuCoor_i booz2_navigation_carrot;
-
-extern struct EnuCoor_f waypoints_float[];
-extern struct EnuCoor_i waypoints[];
-extern const uint8_t nb_waypoint;
-
-extern void booz2_nav_init(void);
-extern void booz2_nav_run(void);
-
-extern uint16_t stage_time, block_time;
-
-extern uint8_t nav_stage, nav_block;
-extern uint8_t last_block, last_stage;
-extern uint8_t last_wp __attribute__ ((unused));
-
-extern int32_t ground_alt;
-
-extern uint8_t horizontal_mode;
-extern uint8_t nav_segment_start, nav_segment_end;
-extern uint8_t nav_circle_centre;
-extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
-#define HORIZONTAL_MODE_WAYPOINT 0
-#define HORIZONTAL_MODE_ROUTE 1
-#define HORIZONTAL_MODE_CIRCLE 2
-#define HORIZONTAL_MODE_ATTITUDE 3
-extern int32_t nav_roll, nav_pitch;
-extern int32_t nav_heading, nav_course;
-extern float nav_radius;
-
-extern uint8_t vertical_mode;
-extern uint32_t nav_throttle;
-extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
-extern float flight_altitude;
-#define VERTICAL_MODE_MANUAL 0
-#define VERTICAL_MODE_CLIMB 1
-#define VERTICAL_MODE_ALT 2
-
-void nav_init_stage(void);
-void nav_init_block(void);
-void nav_goto_block(uint8_t block_id);
-void compute_dist2_to_home(void);
-unit_t nav_reset_reference( void ) __attribute__ ((unused));
-unit_t nav_reset_alt( void ) __attribute__ ((unused));
-void nav_periodic_task(void);
-void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
-bool_t nav_detect_ground(void);
-
-void nav_home(void);
-
-#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) {
kill_throttle = 1; autopilot_motors_on = 0; } FALSE; })
-#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle =
0; autopilot_motors_on = 1; } FALSE; })
-
-#define InitStage() nav_init_stage();
-
-#define Block(x) case x: nav_block=x;
-#define NextBlock() { nav_block++; nav_init_block(); }
-#define GotoBlock(b) { nav_block=b; nav_init_block(); }
-
-#define Stage(s) case s: nav_stage=s;
-#define NextStageAndBreak() { nav_stage++; InitStage(); break; }
-#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); }
-
-#define Label(x) label_ ## x:
-#define Goto(x) { goto label_ ## x; }
-#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0;
FALSE;})
-
-#define And(x, y) ((x) && (y))
-#define Or(x, y) ((x) || (y))
-#define Min(x,y) (x < y ? x : y)
-#define Max(x,y) (x > y ? x : y)
-#define LessThan(_x, _y) ((_x) < (_y))
-#define MoreThan(_x, _y) ((_x) > (_y))
-
-/** Time in s since the entrance in the current block */
-#define NavBlockTime() (block_time)
-
-#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
-#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
-
-#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos);
FALSE; })
-#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1],
waypoints[_wp2]); FALSE; })
-
-#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
-#define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
-#define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z)
-#define Height(_h) (_h)
-
-/** Normalize a degree angle between 0 and 359 */
-#define NormCourse(x) { \
- while (x < 0) x += 360; \
- while (x >= 360) x -= 360; \
-}
-
-/*********** Navigation to waypoint *************************************/
-#define NavGotoWaypoint(_wp) { \
- horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
- INT32_VECT3_COPY( booz2_navigation_target, waypoints[_wp]); \
-}
-
-/*********** Navigation on a circle **************************************/
-extern void nav_circle(uint8_t wp_center, int32_t radius);
-#define NavCircleWaypoint(_center, _radius) { \
- horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
- nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
-}
-
-#define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
-#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 -
nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
-
-/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
-#define NavQdrCloseTo(x) {}
-#define NavCourseCloseTo(x) {}
-
-/*********** Navigation along a line *************************************/
-extern void nav_route(uint8_t wp_start, uint8_t wp_end);
-#define NavSegment(_start, _end) { \
- horizontal_mode = HORIZONTAL_MODE_ROUTE; \
- nav_route(_start, _end); \
-}
-
-bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
-#define NavApproaching(wp, time) nav_approaching_from(wp, 0)
-#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
-
-#define NavGlide(_last_wp, _wp) {}
-
-/** Set the climb control to auto-throttle with the specified pitch
- pre-command */
-#define NavVerticalAutoThrottleMode(_pitch) { \
- nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
-}
-
-/** Set the climb control to auto-pitch with the specified throttle
- pre-command */
-#define NavVerticalAutoPitchMode(_throttle) {}
-
-/** Set the vertical mode to altitude control with the specified altitude
- setpoint and climb pre-command. */
-#define NavVerticalAltitudeMode(_alt, _pre_climb) { \
- vertical_mode = VERTICAL_MODE_ALT; \
- nav_altitude = POS_BFP_OF_REAL(_alt); \
-}
-
-/** Set the vertical mode to climb control with the specified climb setpoint */
-#define NavVerticalClimbMode(_climb) { \
- vertical_mode = VERTICAL_MODE_CLIMB; \
- nav_climb = SPEED_BFP_OF_REAL(_climb); \
-}
-
-/** Set the vertical mode to fixed throttle with the specified setpoint */
-#define NavVerticalThrottleMode(_throttle) { \
- vertical_mode = VERTICAL_MODE_MANUAL; \
- nav_throttle = (200 * ((uint32_t)_throttle) / 9600); \
-}
-
-#define NavHeading(_course) {}
-
-#define NavAttitude(_roll) { \
- horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
- nav_roll = ANGLE_BFP_OF_REAL(_roll); \
-}
-
-#define NavStartDetectGround() ({ autopilot_detect_ground_once = TRUE; FALSE;
})
-#define NavDetectGround() nav_detect_ground()
-
-#define nav_IncreaseShift(x) {}
-
-#define nav_SetNavRadius(x) {}
-
-#define booz2_navigation_SetNavHeading(x) { \
- nav_heading = ANGLE_BFP_OF_REAL(x); \
-}
-
-#define booz2_navigation_SetFlightAltitude(x) { \
- flight_altitude = x; \
- nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \
-}
-
-
-#define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x)
-#define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y)
-#define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
-
-
-extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3
speed_sp, int16_t heading_rate_sp );
-
-#endif /* BOOZ2_NAVIGATION_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/autopilot.c
2010-09-28 19:00:55 UTC (rev 6020)
@@ -26,7 +26,7 @@
#include "booz_radio_control.h"
#include "booz2_commands.h"
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#include <firmwares/rotorcraft/guidance.h>
#include <firmwares/rotorcraft/stabilization.h>
#include "led.h"
@@ -71,7 +71,7 @@
void autopilot_periodic(void) {
- RunOnceEvery(BOOZ2_NAV_PRESCALER, nav_periodic_task());
+ RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
#ifdef FAILSAFE_GROUND_DETECT
if (autopilot_mode == AP_MODE_FAILSAFE && autopilot_detect_ground) {
autopilot_set_mode(AP_MODE_KILL);
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
2010-09-28 19:00:55 UTC (rev 6020)
@@ -29,7 +29,7 @@
#include <firmwares/rotorcraft/stabilization.h>
#include "booz_fms.h"
#include <firmwares/rotorcraft/ins.h>
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#include "airframe.h"
@@ -192,7 +192,7 @@
#endif
}
else {
- INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot);
+ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
#ifdef GUIDANCE_H_USE_REF
b2_gh_update_ref_from_pos_sp(guidance_h_pos_sp);
#endif
@@ -380,7 +380,7 @@
static inline void guidance_h_nav_enter(void) {
- INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, booz2_navigation_carrot);
+ INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
struct Int32Vect2 pos,speed,zero;
INT_VECT2_ZERO(zero);
VECT2_COPY(pos, ins_ltp_pos);
Modified:
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
2010-09-28 19:00:55 UTC (rev 6020)
@@ -30,7 +30,7 @@
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/ahrs.h>
#include "booz_fms.h"
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#include <firmwares/rotorcraft/ins.h>
#include "math/pprz_algebra_int.h"
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/main.c 2010-09-28
19:00:55 UTC (rev 6020)
@@ -114,7 +114,7 @@
booz_fms_init();
autopilot_init();
- booz2_nav_init();
+ nav_init();
guidance_h_init();
guidance_v_init();
stabilization_init();
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c (from
rev 6019, paparazzi3/trunk/sw/airborne/booz/booz2_navigation.c)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.c
2010-09-28 19:00:55 UTC (rev 6020)
@@ -0,0 +1,343 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 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.
+ */
+
+#define NAV_C
+
+#include <firmwares/rotorcraft/navigation.h>
+
+#include "booz/booz2_debug.h"
+#include "booz_gps.h"
+#include <firmwares/rotorcraft/ins.h>
+
+#include <firmwares/rotorcraft/autopilot.h>
+#include "modules.h"
+#include "flight_plan.h"
+
+#include "math/pprz_algebra_int.h"
+
+const uint8_t nb_waypoint = NB_WAYPOINT;
+struct EnuCoor_f waypoints_float[NB_WAYPOINT] = WAYPOINTS;
+struct EnuCoor_i waypoints[NB_WAYPOINT];
+
+struct EnuCoor_i navigation_target;
+struct EnuCoor_i navigation_carrot;
+
+struct EnuCoor_i nav_last_point;
+
+uint16_t stage_time, block_time;
+
+uint8_t nav_stage, nav_block;
+uint8_t last_block, last_stage;
+uint8_t last_wp __attribute__ ((unused));
+
+int32_t ground_alt;
+
+uint8_t horizontal_mode;
+uint8_t nav_segment_start, nav_segment_end;
+uint8_t nav_circle_centre;
+int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
+
+int32_t nav_roll, nav_pitch;
+int32_t nav_heading, nav_course;
+float nav_radius;
+
+#ifndef DEFAULT_CIRCLE_RADIUS
+#define DEFAULT_CIRCLE_RADIUS 0.
+#endif
+
+uint8_t vertical_mode;
+uint32_t nav_throttle;
+int32_t nav_climb, nav_altitude, nav_flight_altitude;
+float flight_altitude;
+
+static inline void nav_set_altitude( void );
+
+#define CLOSE_TO_WAYPOINT (15 << 8)
+#define ARRIVED_AT_WAYPOINT (3 << 8)
+#define CARROT_DIST (12 << 8)
+
+void nav_init(void) {
+ // init int32 waypoints
+ uint8_t i = 0;
+ for (i = 0; i < nb_waypoint; i++) {
+ waypoints[i].x = POS_BFP_OF_REAL(waypoints_float[i].x);
+ waypoints[i].y = POS_BFP_OF_REAL(waypoints_float[i].y);
+ waypoints[i].z = POS_BFP_OF_REAL((waypoints_float[i].z - GROUND_ALT));
+ }
+ nav_block = 0;
+ nav_stage = 0;
+ ground_alt = POS_BFP_OF_REAL(GROUND_ALT);
+ nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
+ nav_flight_altitude = nav_altitude;
+ flight_altitude = SECURITY_ALT;
+ INT32_VECT3_COPY( navigation_target, waypoints[WP_HOME]);
+ INT32_VECT3_COPY( navigation_carrot, waypoints[WP_HOME]);
+
+ horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
+ vertical_mode = VERTICAL_MODE_ALT;
+
+ nav_roll = 0;
+ nav_pitch = 0;
+ nav_heading = 0;
+ nav_course = 0;
+ nav_radius = DEFAULT_CIRCLE_RADIUS;
+ nav_throttle = 0;
+ nav_climb = 0;
+
+}
+
+void nav_run(void) {
+
+ /* compute a vector to the waypoint */
+ struct Int32Vect2 path_to_waypoint;
+ VECT2_DIFF(path_to_waypoint, navigation_target, ins_enu_pos);
+
+ /* saturate it */
+ VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
+
+ int32_t dist_to_waypoint;
+ INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
+
+#ifndef GUIDANCE_H_USE_REF
+ if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
+ VECT2_COPY(navigation_carrot, navigation_target);
+ }
+ else {
+ struct Int32Vect2 path_to_carrot;
+ VECT2_SMUL(path_to_carrot, path_to_waypoint, CARROT_DIST);
+ VECT2_SDIV(path_to_carrot, path_to_carrot, dist_to_waypoint);
+ VECT2_SUM(navigation_carrot, path_to_carrot, ins_enu_pos);
+ }
+#else
+ // if H_REF is used, CARROT_DIST is not used
+ VECT2_COPY(navigation_carrot, navigation_target);
+#endif
+
+ nav_set_altitude();
+}
+
+void nav_circle(uint8_t wp_center, int32_t radius) {
+ if (radius == 0) {
+ VECT2_COPY(navigation_target, waypoints[wp_center]);
+ }
+ else {
+ struct Int32Vect2 pos_diff;
+ VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_center]);
+ // go back to half metric precision or values are too large
+ //INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
+ // store last qdr
+ int32_t last_qdr = nav_circle_qdr;
+ // compute qdr
+ INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
+ // increment circle radians
+ if (nav_circle_radians != 0) {
+ int32_t angle_diff = nav_circle_qdr - last_qdr;
+ INT32_ANGLE_NORMALIZE(angle_diff);
+ nav_circle_radians += angle_diff;
+ }
+ else {
+ // Smallest angle to increment at next step
+ nav_circle_radians = 1;
+ }
+
+ // direction of rotation
+ int8_t sign_radius = radius > 0 ? 1 : -1;
+ // absolute radius
+ int32_t abs_radius = abs(radius);
+ // carrot_angle
+ int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
+ Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
+ carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
+ int32_t s_carrot, c_carrot;
+ PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
+ PPRZ_ITRIG_COS(c_carrot, carrot_angle);
+ // compute setpoint
+ VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
+ INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
+ VECT2_SUM(navigation_target, waypoints[wp_center], pos_diff);
+ }
+ nav_circle_centre = wp_center;
+ nav_circle_radius = radius;
+ horizontal_mode = HORIZONTAL_MODE_CIRCLE;
+}
+
+
+//#include "stdio.h"
+void nav_route(uint8_t wp_start, uint8_t wp_end) {
+ struct Int32Vect2 wp_diff,pos_diff;
+ VECT2_DIFF(wp_diff, waypoints[wp_end],waypoints[wp_start]);
+ VECT2_DIFF(pos_diff, ins_enu_pos,waypoints[wp_start]);
+ // go back to metric precision or values are too large
+ INT32_VECT2_RSHIFT(wp_diff,wp_diff,INT32_POS_FRAC);
+ INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC);
+ int32_t leg_length;
+ int32_t leg_length2 = Max((wp_diff.x * wp_diff.x + wp_diff.y * wp_diff.y),1);
+ INT32_SQRT(leg_length,leg_length2);
+ int32_t nav_leg_progress = (pos_diff.x * wp_diff.x + pos_diff.y * wp_diff.y)
/ leg_length;
+ int32_t progress = Max((CARROT_DIST >> INT32_POS_FRAC), 0);
+ nav_leg_progress += progress;
+ int32_t prog_2 = leg_length;// + progress / 2;
+ Bound(nav_leg_progress, 0, prog_2);
+ struct Int32Vect2 progress_pos;
+ VECT2_SMUL(progress_pos, wp_diff, nav_leg_progress);
+ VECT2_SDIV(progress_pos, progress_pos, leg_length);
+ INT32_VECT2_LSHIFT(progress_pos,progress_pos,INT32_POS_FRAC);
+ VECT2_SUM(navigation_target,waypoints[wp_start],progress_pos);
+ //printf("target %d %d | p %d %d | s %d %d | l %d %d %d\n",
+ // navigation_target.x,
+ // navigation_target.y,
+ // progress_pos.x,
+ // progress_pos.y,
+ // waypoints[wp_start].x,
+ // waypoints[wp_start].y,
+ // leg_length, leg_length2, nav_leg_progress);
+ //fflush(stdout);
+
+ nav_segment_start = wp_start;
+ nav_segment_end = wp_end;
+ horizontal_mode = HORIZONTAL_MODE_ROUTE;
+}
+
+bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
+ int32_t dist_to_point;
+ struct Int32Vect2 diff;
+ static uint8_t time_at_wp = 0;
+ VECT2_DIFF(diff, waypoints[wp_idx], ins_enu_pos);
+ INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
+ INT32_VECT2_NORM(dist_to_point, diff);
+ //printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
+ //fflush(stdout);
+ //if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
+ if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
+ else time_at_wp = 0;
+ if (time_at_wp > 20) return TRUE;
+ if (from_idx > 0 && from_idx < NB_WAYPOINT) {
+ struct Int32Vect2 from_diff;
+ VECT2_DIFF(from_diff, waypoints[wp_idx],waypoints[from_idx]);
+ INT32_VECT2_RSHIFT(from_diff,from_diff,INT32_POS_FRAC);
+ return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
+ }
+ else return FALSE;
+}
+
+static inline void nav_set_altitude( void ) {
+ static int32_t last_nav_alt = 0;
+ if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
+ nav_flight_altitude = nav_altitude;
+ last_nav_alt = nav_altitude;
+ }
+}
+
+/** Reset the geographic reference to the current GPS fix */
+unit_t nav_reset_reference( void ) {
+ ins_ltp_initialised = FALSE;
+ ins_hf_realign = TRUE;
+ ins_vf_realign = TRUE;
+ return 0;
+}
+
+unit_t nav_reset_alt( void ) {
+ ins_vf_realign = TRUE;
+
+#ifdef USE_GPS
+ ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
+ ins_ltp_def.hmsl = booz_gps_state.hmsl;
+#endif
+
+ return 0;
+}
+
+void nav_init_stage( void ) {
+ INT32_VECT3_COPY(nav_last_point, ins_enu_pos);
+ stage_time = 0;
+ nav_circle_radians = 0;
+ horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
+}
+
+void nav_init_block(void) {
+ if (nav_block >= NB_BLOCK)
+ nav_block=NB_BLOCK-1;
+ nav_stage = 0;
+ block_time = 0;
+ InitStage();
+}
+
+void nav_goto_block(uint8_t b) {
+ if (b != nav_block) { /* To avoid a loop in a the current block */
+ last_block = nav_block;
+ last_stage = nav_stage;
+ }
+ GotoBlock(b);
+}
+
+#include <stdio.h>
+void nav_periodic_task() {
+ RunOnceEvery(16, { stage_time++; block_time++; });
+
+ /* from flight_plan.h */
+ auto_nav();
+
+ /* run carrot loop */
+ nav_run();
+
+ ground_alt = POS_BFP_OF_REAL((float)ins_ltp_def.hmsl / 100.);
+
+}
+
+#include "downlink.h"
+#include "messages.h"
+#include "uart.h"
+void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
+ if (wp_id < nb_waypoint) {
+ INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
+ DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp_id, &(new_pos->x),
&(new_pos->y), &(new_pos->z));
+ }
+}
+
+void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp,
int16_t heading_rate_sp ) {
+ MY_ASSERT(wp < nb_waypoint);
+ int32_t s_heading, c_heading;
+ PPRZ_ITRIG_SIN(s_heading, nav_heading);
+ PPRZ_ITRIG_COS(c_heading, nav_heading);
+ // FIXME : scale POS to SPEED
+ struct Int32Vect3 delta_pos;
+ VECT3_SDIV(delta_pos, speed_sp,NAV_FREQ); /* fixme :make sure the division
is really a >> */
+ INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC-INT32_POS_FRAC));
+ waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >>
INT32_TRIG_FRAC;
+ waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >>
INT32_TRIG_FRAC;
+ waypoints[wp].z += delta_pos.z;
+ int32_t delta_heading = heading_rate_sp / NAV_FREQ;
+ delta_heading = delta_heading >> (INT32_SPEED_FRAC-INT32_POS_FRAC);
+ nav_heading += delta_heading;
+
+ INT32_COURSE_NORMALIZE(nav_heading);
+ RunOnceEvery(10,DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, &wp,
&(waypoints[wp].x), &(waypoints[wp].y), &(waypoints[wp].z)));
+}
+
+bool_t nav_detect_ground(void) {
+ if (!autopilot_detect_ground) return FALSE;
+ autopilot_detect_ground = FALSE;
+ return TRUE;
+}
+
+void nav_home(void) {}
Copied: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.h (from
rev 6019, paparazzi3/trunk/sw/airborne/booz/booz2_navigation.h)
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/navigation.h
2010-09-28 19:00:55 UTC (rev 6020)
@@ -0,0 +1,222 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2008-2009 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 NAVIGATION_H
+#define NAVIGATION_H
+
+#include "std.h"
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
+
+#define NAV_FREQ 16
+// FIXME use periodic FREQ
+#define NAV_PRESCALER (512/NAV_FREQ)
+
+extern struct EnuCoor_i navigation_target;
+extern struct EnuCoor_i navigation_carrot;
+
+extern struct EnuCoor_f waypoints_float[];
+extern struct EnuCoor_i waypoints[];
+extern const uint8_t nb_waypoint;
+
+extern void nav_init(void);
+extern void nav_run(void);
+
+extern uint16_t stage_time, block_time;
+
+extern uint8_t nav_stage, nav_block;
+extern uint8_t last_block, last_stage;
+extern uint8_t last_wp __attribute__ ((unused));
+
+extern int32_t ground_alt;
+
+extern uint8_t horizontal_mode;
+extern uint8_t nav_segment_start, nav_segment_end;
+extern uint8_t nav_circle_centre;
+extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians;
+#define HORIZONTAL_MODE_WAYPOINT 0
+#define HORIZONTAL_MODE_ROUTE 1
+#define HORIZONTAL_MODE_CIRCLE 2
+#define HORIZONTAL_MODE_ATTITUDE 3
+extern int32_t nav_roll, nav_pitch;
+extern int32_t nav_heading, nav_course;
+extern float nav_radius;
+
+extern uint8_t vertical_mode;
+extern uint32_t nav_throttle;
+extern int32_t nav_climb, nav_altitude, nav_flight_altitude;
+extern float flight_altitude;
+#define VERTICAL_MODE_MANUAL 0
+#define VERTICAL_MODE_CLIMB 1
+#define VERTICAL_MODE_ALT 2
+
+void nav_init_stage(void);
+void nav_init_block(void);
+void nav_goto_block(uint8_t block_id);
+void compute_dist2_to_home(void);
+unit_t nav_reset_reference( void ) __attribute__ ((unused));
+unit_t nav_reset_alt( void ) __attribute__ ((unused));
+void nav_periodic_task(void);
+void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
+bool_t nav_detect_ground(void);
+
+void nav_home(void);
+
+#define NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) {
kill_throttle = 1; autopilot_motors_on = 0; } FALSE; })
+#define NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { kill_throttle =
0; autopilot_motors_on = 1; } FALSE; })
+
+#define InitStage() nav_init_stage();
+
+#define Block(x) case x: nav_block=x;
+#define NextBlock() { nav_block++; nav_init_block(); }
+#define GotoBlock(b) { nav_block=b; nav_init_block(); }
+
+#define Stage(s) case s: nav_stage=s;
+#define NextStageAndBreak() { nav_stage++; InitStage(); break; }
+#define NextStageAndBreakFrom(wp) { last_wp = wp; NextStageAndBreak(); }
+
+#define Label(x) label_ ## x:
+#define Goto(x) { goto label_ ## x; }
+#define Return() ({ nav_block=last_block; nav_stage=last_stage; block_time=0;
FALSE;})
+
+#define And(x, y) ((x) && (y))
+#define Or(x, y) ((x) || (y))
+#define Min(x,y) (x < y ? x : y)
+#define Max(x,y) (x > y ? x : y)
+#define LessThan(_x, _y) ((_x) < (_y))
+#define MoreThan(_x, _y) ((_x) > (_y))
+
+/** Time in s since the entrance in the current block */
+#define NavBlockTime() (block_time)
+
+#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
+#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
+
+#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], ins_enu_pos);
FALSE; })
+#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1],
waypoints[_wp2]); FALSE; })
+
+#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
+#define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
+#define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z)
+#define Height(_h) (_h)
+
+/** Normalize a degree angle between 0 and 359 */
+#define NormCourse(x) { \
+ while (x < 0) x += 360; \
+ while (x >= 360) x -= 360; \
+}
+
+/*********** Navigation to waypoint *************************************/
+#define NavGotoWaypoint(_wp) { \
+ horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
+ INT32_VECT3_COPY( navigation_target, waypoints[_wp]); \
+}
+
+/*********** Navigation on a circle **************************************/
+extern void nav_circle(uint8_t wp_center, int32_t radius);
+#define NavCircleWaypoint(_center, _radius) { \
+ horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
+ nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
+}
+
+#define NavCircleCount() (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
+#define NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 -
nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
+
+/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
+#define NavQdrCloseTo(x) {}
+#define NavCourseCloseTo(x) {}
+
+/*********** Navigation along a line *************************************/
+extern void nav_route(uint8_t wp_start, uint8_t wp_end);
+#define NavSegment(_start, _end) { \
+ horizontal_mode = HORIZONTAL_MODE_ROUTE; \
+ nav_route(_start, _end); \
+}
+
+bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
+#define NavApproaching(wp, time) nav_approaching_from(wp, 0)
+#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
+
+#define NavGlide(_last_wp, _wp) {}
+
+/** Set the climb control to auto-throttle with the specified pitch
+ pre-command */
+#define NavVerticalAutoThrottleMode(_pitch) { \
+ nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
+}
+
+/** Set the climb control to auto-pitch with the specified throttle
+ pre-command */
+#define NavVerticalAutoPitchMode(_throttle) {}
+
+/** Set the vertical mode to altitude control with the specified altitude
+ setpoint and climb pre-command. */
+#define NavVerticalAltitudeMode(_alt, _pre_climb) { \
+ vertical_mode = VERTICAL_MODE_ALT; \
+ nav_altitude = POS_BFP_OF_REAL(_alt); \
+}
+
+/** Set the vertical mode to climb control with the specified climb setpoint */
+#define NavVerticalClimbMode(_climb) { \
+ vertical_mode = VERTICAL_MODE_CLIMB; \
+ nav_climb = SPEED_BFP_OF_REAL(_climb); \
+}
+
+/** Set the vertical mode to fixed throttle with the specified setpoint */
+#define NavVerticalThrottleMode(_throttle) { \
+ vertical_mode = VERTICAL_MODE_MANUAL; \
+ nav_throttle = (200 * ((uint32_t)_throttle) / 9600); \
+}
+
+#define NavHeading(_course) {}
+
+#define NavAttitude(_roll) { \
+ horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
+ nav_roll = ANGLE_BFP_OF_REAL(_roll); \
+}
+
+#define NavStartDetectGround() ({ autopilot_detect_ground_once = TRUE; FALSE;
})
+#define NavDetectGround() nav_detect_ground()
+
+#define nav_IncreaseShift(x) {}
+
+#define nav_SetNavRadius(x) {}
+
+#define navigation_SetNavHeading(x) { \
+ nav_heading = ANGLE_BFP_OF_REAL(x); \
+}
+
+#define navigation_SetFlightAltitude(x) { \
+ flight_altitude = x; \
+ nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude) - ground_alt; \
+}
+
+
+#define GetPosX() POS_FLOAT_OF_BFP(ins_enu_pos.x)
+#define GetPosY() POS_FLOAT_OF_BFP(ins_enu_pos.y)
+#define GetPosAlt() (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
+
+
+extern void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3
speed_sp, int16_t heading_rate_sp );
+
+#endif /* NAVIGATION_H */
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-09-28 16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-09-28 19:00:55 UTC (rev 6020)
@@ -660,7 +660,7 @@
}
#include "booz_gps.h"
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#define PERIODIC_SEND_BOOZ2_FP(_chan) {
\
int32_t carrot_up = -guidance_v_z_sp; \
DOWNLINK_SEND_BOOZ2_FP( _chan, \
@@ -704,9 +704,9 @@
#define PERIODIC_SEND_BOOZ2_GPS(_chan) {}
#endif
-#include "booz2_navigation.h"
-#define PERIODIC_SEND_BOOZ2_NAV_STATUS(_chan) {
\
- DOWNLINK_SEND_BOOZ2_NAV_STATUS(_chan, \
+#include <firmwares/rotorcraft/navigation.h>
+#define PERIODIC_SEND_ROTORCRAFT_NAV_STATUS(_chan) {
\
+ DOWNLINK_SEND_ROTORCRAFT_NAV_STATUS(_chan, \
&block_time, \
&stage_time, \
&nav_block, \
Modified: paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/modules/cam_control/booz_cam.c 2010-09-28
19:00:55 UTC (rev 6020)
@@ -25,7 +25,7 @@
#include "cam_control/booz_cam.h"
#include "booz2_pwm_hw.h"
#include <firmwares/rotorcraft/ahrs.h>
-#include "booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
#include <firmwares/rotorcraft/ins.h>
#include "flight_plan.h"
Modified: paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/airborne/modules/vehicle_interface/vi.h 2010-09-28
19:00:55 UTC (rev 6020)
@@ -33,7 +33,7 @@
#include <firmwares/rotorcraft/autopilot.h>
#include <firmwares/rotorcraft/stabilization.h>
#include <firmwares/rotorcraft/guidance.h>
-#include "booz/booz2_navigation.h"
+#include <firmwares/rotorcraft/navigation.h>
struct Vi_imu_info {
struct Int16Vect3 gyro;
Modified: paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml
===================================================================
--- paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml 2010-09-28
16:56:07 UTC (rev 6019)
+++ paparazzi3/trunk/sw/ground_segment/tmtc/booz_server.ml 2010-09-28
19:00:55 UTC (rev 6020)
@@ -2,7 +2,7 @@
* $Id: fw_server.ml,v 1.1 2009/03/22 17:53:48 hecto Exp $
*
* Server part specific to booz vehicles
- *
+ *
* Copyright (C) ENAC
*
* This file is part of paparazzi.
@@ -20,7 +20,7 @@
* 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.
+ * Boston, MA 02111-1307, USA.
*
*)
@@ -46,16 +46,16 @@
let fvalue = fun x ->
match x with
- Pprz.Float x -> x
- | Pprz.Int32 x -> Int32.to_float x
- | Pprz.Int x -> float_of_int x
+ Pprz.Float x -> x
+ | Pprz.Int32 x -> Int32.to_float x
+ | Pprz.Int x -> float_of_int x
| _ -> failwith (sprintf "Receive.log_and_parse: float expected, got '%s'"
(Pprz.string_of_value x))
-
+
let ivalue = fun x ->
match x with
Pprz.Int x -> x
- | Pprz.Int32 x -> Int32.to_int x
+ | Pprz.Int32 x -> Int32.to_int x
| _ -> failwith "Receive.log_and_parse: int expected"
(*
@@ -133,13 +133,13 @@
let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let value = fun x -> try Pprz.assoc x values with Not_found -> failwith
(sprintf "Error: field '%s' not found\n" x) in
- let fvalue = fun x ->
+ let fvalue = fun x ->
let f = fvalue (value x) in
match classify_float f with
- FP_infinite | FP_nan ->
- let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name
msg.Pprz.name (string_of_values values) in
- raise (Telemetry_error (ac_name, format_string_field msg))
-
+ FP_infinite | FP_nan ->
+ let msg = sprintf "Non normal number: %f in '%s %s %s'" f ac_name
msg.Pprz.name (string_of_values values) in
+ raise (Telemetry_error (ac_name, format_string_field msg))
+
| _ -> f
and ivalue = fun x -> ivalue (value x)
(*and i32value = fun x -> i32value (value x)*)
@@ -179,7 +179,7 @@
a.itow <- Int32.of_float (fvalue "itow");*)
a.flight_time <- ivalue "flight_time";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
- Wind.update ac_name a.gspeed a.course
+ Wind.update ac_name a.gspeed a.course
| "BOOZ_STATUS" ->
a.fbw.rc_status <- get_rc_status (ivalue "rc_status");
a.gps_mode <- check_index (ivalue "gps_status") gps_modes
"GPS_MODE";
@@ -195,7 +195,7 @@
let nav_ref_ecef = LL.make_ecef [| x; y; z |] in
a.nav_ref <- Some (Ltp nav_ref_ecef);
a.d_hmsl <- hmsl -. alt;
- | "BOOZ2_NAV_STATUS" ->
+ | "ROTORCRAFT_NAV_STATUS" ->
a.block_time <- ivalue "block_time";
a.stage_time <- ivalue "stage_time";
a.cur_block <- ivalue "cur_block";
@@ -214,4 +214,3 @@
| None -> (); (** Can't use this message *)
end
| _ -> ()
-
Modified: paparazzi3/trunk/sw/logalizer/export.ml
===================================================================
--- paparazzi3/trunk/sw/logalizer/export.ml 2010-09-28 16:56:07 UTC (rev
6019)
+++ paparazzi3/trunk/sw/logalizer/export.ml 2010-09-28 19:00:55 UTC (rev
6020)
@@ -93,11 +93,11 @@
and utm_north = float_of_string (lookup "GPS" "utm_north") /. 100.
and utm_zone = int_of_string (lookup "GPS" "utm_zone") in
Latlong.of_utm WGS84 {utm_x=utm_east; utm_y=utm_north; utm_zone=utm_zone}
- else if lookup "BOOZ2_NAV_REF" "x" <>"" && lookup "BOOZ2_FP" "east" <>"" then
+ else if lookup "NAV_REF" "x" <>"" && lookup "BOOZ2_FP" "east" <>"" then
let getf = fun m f -> float_of_string (lookup m f) in
- let x0 = getf "BOOZ2_NAV_REF" "x" /. 100.
- and y0 = getf "BOOZ2_NAV_REF" "y" /. 100.
- and z0 = getf "BOOZ2_NAV_REF" "z" /. 100.
+ let x0 = getf "NAV_REF" "x" /. 100.
+ and y0 = getf "NAV_REF" "y" /. 100.
+ and z0 = getf "NAV_REF" "z" /. 100.
and e = getf "BOOZ2_FP" "east" /. 256.
and n = getf "BOOZ2_FP" "north" /. 256.
and u = getf "BOOZ2_FP" "up" /. 256. in
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6020] move and rename booz2_navigation to rotorcraft navigation,
Felix Ruess <=