diff --git a/conf/firmwares/subsystems/rotorcraft/navigation.makefile b/conf/firmwares/subsystems/rotorcraft/navigation.makefile index 2890d3b271b..49e29abc30f 100644 --- a/conf/firmwares/subsystems/rotorcraft/navigation.makefile +++ b/conf/firmwares/subsystems/rotorcraft/navigation.makefile @@ -2,4 +2,5 @@ $(TARGET).CFLAGS += -DUSE_NAVIGATION $(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c +$(TARGET).srcs += subsystems/navigation/waypoints.c $(TARGET).srcs += subsystems/navigation/common_flight_plan.c diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index 187f37462d8..b4520fd23bb 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -48,9 +48,6 @@ #include "messages.h" #include "mcu_periph/uart.h" -const uint8_t nb_waypoint = NB_WAYPOINT; -struct EnuCoor_i waypoints[NB_WAYPOINT]; - struct EnuCoor_i navigation_target; struct EnuCoor_i navigation_carrot; @@ -136,30 +133,23 @@ static void send_wp_moved(struct transport_tx *trans, struct link_device *dev) if (i >= nb_waypoint) { i = 0; } pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID, &i, - &(waypoints[i].x), - &(waypoints[i].y), - &(waypoints[i].z)); + &(waypoints[i].enu_i.x), + &(waypoints[i].enu_i.y), + &(waypoints[i].enu_i.z)); } #endif void nav_init(void) { - // convert to - const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; - // init int32 waypoints - uint8_t i = 0; - for (i = 0; i < nb_waypoint; i++) { - waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x); - waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y); - waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z); - } + waypoints_init(); + nav_block = 0; nav_stage = 0; nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT); nav_flight_altitude = nav_altitude; flight_altitude = SECURITY_ALT; - VECT3_COPY(navigation_target, waypoints[WP_HOME]); - VECT3_COPY(navigation_carrot, waypoints[WP_HOME]); + VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i); + VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i); horizontal_mode = HORIZONTAL_MODE_WAYPOINT; vertical_mode = VERTICAL_MODE_ALT; @@ -383,12 +373,15 @@ static inline void nav_set_altitude(void) unit_t nav_reset_reference(void) { ins_reset_local_origin(); + /* update local ENU coordinates of global waypoints */ + nav_localize_global_waypoints(); return 0; } unit_t nav_reset_alt(void) { ins_reset_altitude_ref(); + nav_localize_global_waypoints(); return 0; } @@ -414,28 +407,6 @@ void nav_periodic_task(void) nav_run(); } -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos) -{ - if (stateIsLocalCoordinateValid()) { - struct EnuCoor_i enu; - enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos); - // convert ENU pos from cm to BFP with INT32_POS_FRAC - enu.x = POS_BFP_OF_REAL(enu.x) / 100; - enu.y = POS_BFP_OF_REAL(enu.y) / 100; - enu.z = POS_BFP_OF_REAL(enu.z) / 100; - nav_move_waypoint(wp_id, &enu); - } -} - -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos) -{ - if (wp_id < nb_waypoint) { - VECT3_COPY(waypoints[wp_id], (*new_pos)); - DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &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); FIXME @@ -446,16 +417,18 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int 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; + waypoints[wp].enu_i.x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[wp].enu_i.y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC; + waypoints[wp].enu_i.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, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y), - &(waypoints[wp].z))); + RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, + &(waypoints[wp].enu_i.x), + &(waypoints[wp].enu_i.y), + &(waypoints[wp].enu_i.z))); } bool_t nav_detect_ground(void) @@ -474,10 +447,10 @@ bool_t nav_is_in_flight(void) void nav_home(void) { horizontal_mode = HORIZONTAL_MODE_WAYPOINT; - VECT3_COPY(navigation_target, waypoints[WP_HOME]); + VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i); vertical_mode = VERTICAL_MODE_ALT; - nav_altitude = waypoints[WP_HOME].z; + nav_altitude = waypoints[WP_HOME].enu_i.z; nav_flight_altitude = nav_altitude; dist2_to_wp = dist2_to_home; @@ -499,7 +472,7 @@ float get_dist2_to_point(struct EnuCoor_i *p) /** Returns squared horizontal distance to given waypoint */ float get_dist2_to_waypoint(uint8_t wp_id) { - return get_dist2_to_point(&waypoints[wp_id]); + return get_dist2_to_point(&waypoints[wp_id].enu_i); } /** Computes squared distance to the HOME waypoint potentially sets diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 16930a506ab..d0a809a769b 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -30,8 +30,8 @@ #include "std.h" #include "math/pprz_geodetic_int.h" -#include "math/pprz_geodetic_float.h" +#include "subsystems/navigation/waypoints.h" #include "subsystems/navigation/common_flight_plan.h" #define NAV_FREQ 16 @@ -39,9 +39,6 @@ extern struct EnuCoor_i navigation_target; extern struct EnuCoor_i navigation_carrot; -extern struct EnuCoor_i waypoints[]; -extern const uint8_t nb_waypoint; - extern void nav_init(void); extern void nav_run(void); @@ -84,8 +81,6 @@ extern void nav_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_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos); -void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos); bool_t nav_detect_ground(void); bool_t nav_is_in_flight(void); @@ -107,13 +102,7 @@ extern bool_t nav_set_heading_current(void); #define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; }) #define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; }) -#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); 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) +#define NavSetWaypointHere(_wp) ({ nav_set_waypoint_here_2d(_wp); FALSE; }) /** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ @@ -124,7 +113,7 @@ extern bool_t nav_set_heading_current(void); /*********** Navigation to waypoint *************************************/ #define NavGotoWaypoint(_wp) { \ horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \ - VECT3_COPY(navigation_target, waypoints[_wp]); \ + VECT3_COPY(navigation_target, waypoints[_wp].enu_i); \ dist2_to_wp = get_dist2_to_waypoint(_wp); \ } @@ -132,7 +121,7 @@ extern bool_t nav_set_heading_current(void); extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius); #define NavCircleWaypoint(_center, _radius) { \ horizontal_mode = HORIZONTAL_MODE_CIRCLE; \ - nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \ + nav_circle(&waypoints[_center].enu_i, POS_BFP_OF_REAL(_radius)); \ } #define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI) @@ -146,25 +135,25 @@ extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius); extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end); #define NavSegment(_start, _end) { \ horizontal_mode = HORIZONTAL_MODE_ROUTE; \ - nav_route(&waypoints[_start], &waypoints[_end]); \ + nav_route(&waypoints[_start].enu_i, &waypoints[_end].enu_i); \ } /** Nav glide routine */ #define NavGlide(_last_wp, _wp) { \ - int32_t start_alt = waypoints[_last_wp].z; \ - int32_t diff_alt = waypoints[_wp].z - start_alt; \ + int32_t start_alt = waypoints[_last_wp].enu_i.z; \ + int32_t diff_alt = waypoints[_wp].enu_i.z - start_alt; \ int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \ NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \ } /** Proximity tests on approaching a wp */ bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time); -#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time) -#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time) +#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time) +#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time) /** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */ bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time); -#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time) +#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time) /** Set the climb control to auto-throttle with the specified pitch pre-command */ diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c new file mode 100644 index 00000000000..e2765bc91bf --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -0,0 +1,227 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * 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, see + * . + */ + +/** + * @file subsystems/navigation/waypoints.c + * + */ + +#include "subsystems/navigation/waypoints.h" +#include "state.h" +#include "subsystems/datalink/downlink.h" +#include "generated/flight_plan.h" + +const uint8_t nb_waypoint = NB_WAYPOINT; +struct Waypoint waypoints[NB_WAYPOINT]; + +/** initialize global and local waypoints */ +void waypoints_init(void) +{ + struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU; + struct LlaCoor_i wp_tmp_lla_i[NB_WAYPOINT] = WAYPOINTS_LLA_WGS84; + /* element in array is TRUE if absolute/global waypoint */ + bool_t is_global[NB_WAYPOINT] = WAYPOINTS_GLOBAL; + uint8_t i = 0; + for (i = 0; i < nb_waypoint; i++) { + /* clear all flags */ + waypoints[i].flags = 0; + /* init waypoint as global LLA or local ENU */ + if (is_global[i]) { + waypoint_set_global_flag(i); + nav_set_waypoint_lla(i, &wp_tmp_lla_i[i]); + } else { + nav_set_waypoint_enu_f(i, &wp_tmp_float[i]); + } + } +} + +void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu) +{ + if (wp_id < nb_waypoint) { + memcpy(&waypoints[wp_id].enu_i, enu, sizeof(struct EnuCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_globalize_local_wp(wp_id); + } +} + +void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu) +{ + if (wp_id < nb_waypoint) { + memcpy(&waypoints[wp_id].enu_f, enu, sizeof(struct EnuCoor_f)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_BFP_OF_REAL(waypoints[wp_id].enu_i, waypoints[wp_id].enu_f); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + ClearBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_globalize_local_wp(wp_id); + } +} + +void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos) +{ + if (wp_id < nb_waypoint) { + nav_set_waypoint_enu_i(wp_id, new_pos); + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), + &(new_pos->y), &(new_pos->z)); + } +} + +/** + * Set only local XY coordinates of waypoint without update altitude. + * @TODO: how to handle global waypoints? + */ +void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t y) +{ + if (wp_id < nb_waypoint) { + waypoints[wp_id].enu_i.x = x; + waypoints[wp_id].enu_i.y = y; + /* also update ENU float representation */ + waypoints[wp_id].enu_f.x = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.x); + waypoints[wp_id].enu_f.y = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.y); + nav_globalize_local_wp(wp_id); + } +} + +void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + memcpy(&waypoints[wp_id].lla, lla, sizeof(struct LlaCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_localize_global_wp(wp_id); +} + +void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + nav_set_waypoint_lla(wp_id, lla); + if (nav_wp_is_global(wp_id)) { + /* lla->alt is above ellipsoid, WP_MOVED_LLA has hmsl alt */ + int32_t hmsl = lla->alt - state.ned_origin_i.lla.alt + state.ned_origin_i.hmsl; + DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id, + &lla->lat, &lla->lon, &hmsl); + } else { + DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, + &waypoints[wp_id].enu_i.x, + &waypoints[wp_id].enu_i.y, + &waypoints[wp_id].enu_i.z); + } +} + +/** set waypoint latitude/longitude without updating altitude */ +void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla) +{ + if (wp_id >= nb_waypoint) { + return; + } + waypoints[wp_id].lla.lat = lla->lat; + waypoints[wp_id].lla.lon = lla->lon; + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + nav_localize_global_wp(wp_id); +} + +/** set waypoint to current location and altitude */ +void nav_set_waypoint_here(uint8_t wp_id) +{ + if (wp_id >= nb_waypoint) { + return; + } + if (nav_wp_is_global(wp_id)) { + nav_set_waypoint_lla(wp_id, stateGetPositionLla_i()); + } else { + nav_set_waypoint_enu_i(wp_id, stateGetPositionEnu_i()); + } +} + +/** set waypoint to current horizontal location without modifying altitude */ +void nav_set_waypoint_here_2d(uint8_t wp_id) +{ + if (wp_id >= nb_waypoint) { + return; + } + if (nav_wp_is_global(wp_id)) { + nav_set_waypoint_latlon(wp_id, stateGetPositionLla_i()); + } else { + nav_set_waypoint_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y); + } +} + +void nav_globalize_local_wp(uint8_t wp_id) +{ + if (state.ned_initialized_i) { + struct EcefCoor_i ecef; + ecef_of_enu_pos_i(&ecef, &state.ned_origin_i, &waypoints[wp_id].enu_i); + lla_of_ecef_i(&waypoints[wp_id].lla, &ecef); + SetBit(waypoints[wp_id].flags, WP_FLAG_LLA_I); + } +} + +/** update local ENU coordinates from its LLA coordinates */ +void nav_localize_global_wp(uint8_t wp_id) +{ + if (state.ned_initialized_i) { + struct EnuCoor_i enu; + enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla); + // convert ENU pos from cm to BFP with INT32_POS_FRAC + enu.x = POS_BFP_OF_REAL(enu.x) / 100; + enu.y = POS_BFP_OF_REAL(enu.y) / 100; + enu.z = POS_BFP_OF_REAL(enu.z) / 100; + memcpy(&waypoints[wp_id].enu_i, &enu, sizeof(struct EnuCoor_i)); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I); + ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i); + SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_F); + } +} + +/** update local ENU coordinates of global waypoints */ +void nav_localize_global_waypoints(void) +{ + uint8_t i = 0; + for (i = 0; i < nb_waypoint; i++) { + if (nav_wp_is_global(i)) { + nav_localize_global_wp(i); + } + } +} + +/** Get LLA coordinates of waypoint. + * If the waypoint does not have its global coordinates set, + * the LLA representation is computed if the local origin is set. + * + * @param wp_id waypoint id + * @return pointer to waypoint LLA coordinates, NULL if invalid + */ +struct LlaCoor_i *nav_get_waypoint_lla(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + if (!bit_is_set(waypoints[wp_id].flags, WP_FLAG_LLA_I)) { + nav_globalize_local_wp(wp_id); + } + return &waypoints[wp_id].lla; + } + else { + return NULL; + } +} diff --git a/sw/airborne/subsystems/navigation/waypoints.h b/sw/airborne/subsystems/navigation/waypoints.h new file mode 100644 index 00000000000..ce06a67848e --- /dev/null +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2015 Felix Ruess + * + * 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, see + * . + */ + +/** + * @file subsystems/navigation/waypoints.h + * + */ + +#ifndef WAYPOINTS_H +#define WAYPOINTS_H + +#include "std.h" +#include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" + +#define WP_FLAG_GLOBAL 0 +#define WP_FLAG_ENU_I 1 +#define WP_FLAG_ENU_F 2 +#define WP_FLAG_LLA_I 3 + +struct Waypoint { + uint8_t flags; ///< bitmask encoding valid representations and if local or global + struct EnuCoor_i enu_i; ///< with #INT32_POS_FRAC + struct EnuCoor_f enu_f; + struct LlaCoor_i lla; +}; + +extern const uint8_t nb_waypoint; +/** size == nb_waypoint, waypoint 0 is a dummy waypoint */ +extern struct Waypoint waypoints[]; + +#define WaypointX(_wp) waypoints[_wp].enu_f.x +#define WaypointY(_wp) waypoints[_wp].enu_f.y +#define WaypointAlt(_wp) waypoints[_wp].enu_f.z +#define Height(_h) (_h) + +static inline bool_t nav_wp_is_global(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + return bit_is_set(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } + return FALSE; +} + +static inline void waypoint_set_global_flag(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + SetBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } +} + +static inline void waypoint_clear_global_flag(uint8_t wp_id) +{ + if (wp_id < nb_waypoint) { + ClearBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL); + } +} + +extern void waypoints_init(void); + +extern void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu); +extern void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu); +extern void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t y); +extern void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos); +extern void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla); +extern void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla); +/** set waypoint latitude/longitude without updating altitude */ +void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla); + +/** set waypoint to current location and altitude */ +extern void nav_set_waypoint_here(uint8_t wp_id); + +/** set waypoint to current horizontal location without modifying altitude */ +extern void nav_set_waypoint_here_2d(uint8_t wp_id); + +/** update global LLA coordinates from its ENU coordinates */ +extern void nav_globalize_local_wp(uint8_t wp_id); + +/** update local ENU coordinates from its LLA coordinates */ +extern void nav_localize_global_wp(uint8_t wp_id); +/** update local ENU coordinates of global waypoints */ +extern void nav_localize_global_waypoints(void); + +/** Get LLA coordinates of waypoint. + * If the waypoint does not have its global coordinates set, + * the LLA representation is computed if the local origin is set. + * + * @param wp_id waypoint id + * @return pointer to waypoint LLA coordinates, NULL if invalid + */ +extern struct LlaCoor_i *nav_get_waypoint_lla(uint8_t wp_id); + +#endif /* WAYPOINTS_H */ diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index a1ce64e7f38..93e9d1850c4 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -144,6 +144,11 @@ let print_waypoint_lla_wgs84 = fun utm0 default_alt waypoint -> let alt = float_of_string alt +. Egm96.of_wgs84 wgs84 in printf " {.lat=%Ld, .lon=%Ld, .alt=%.0f}, /* 1e7deg, 1e7deg, mm (above WGS84 ref ellipsoid) */ \\\n" (convert_angle wgs84.posn_lat) (convert_angle wgs84.posn_long) (1000. *. alt) +let print_waypoint_global = fun waypoint -> + try + let (_, _) = (float_attrib waypoint "lat", float_attrib waypoint "lon") in + printf " TRUE, \\\n" + with _ -> printf " FALSE, \\\n" let get_index_block = fun x -> try @@ -848,6 +853,9 @@ let () = Xml2h.define "WAYPOINTS_LLA_WGS84" "{ \\"; List.iter (print_waypoint_lla_wgs84 utm0 alt) waypoints; lprintf "};\n"; + Xml2h.define "WAYPOINTS_GLOBAL" "{ \\"; + List.iter print_waypoint_global waypoints; + lprintf "};\n"; Xml2h.define "NB_WAYPOINT" (string_of_int (List.length waypoints)); Xml2h.define "FP_BLOCKS" "{ \\";