From ea76fa3e64bb8c089a62106b09b50fa050981959 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 20 Apr 2015 14:40:53 +0200 Subject: [PATCH] [waypoints] renamve nav_foo_waypoint_bar to functions to waypoint_foo_bar --- sw/airborne/firmwares/fixedwing/datalink.c | 2 +- sw/airborne/firmwares/rotorcraft/datalink.c | 2 +- sw/airborne/firmwares/rotorcraft/navigation.c | 4 +- sw/airborne/firmwares/rotorcraft/navigation.h | 2 +- sw/airborne/subsystems/navigation/waypoints.c | 72 +++++++++---------- sw/airborne/subsystems/navigation/waypoints.h | 32 ++++----- sw/simulator/nps/nps_ivy_rotorcraft.c | 2 +- 7 files changed, 58 insertions(+), 58 deletions(-) diff --git a/sw/airborne/firmwares/fixedwing/datalink.c b/sw/airborne/firmwares/fixedwing/datalink.c index ec878396851..7804df1357d 100644 --- a/sw/airborne/firmwares/fixedwing/datalink.c +++ b/sw/airborne/firmwares/fixedwing/datalink.c @@ -131,7 +131,7 @@ void dl_parse_msg(void) struct UtmCoor_f utm; utm.zone = nav_utm_zone0; utm_of_lla_f(&utm, &lla); - nav_move_waypoint(wp_id, utm.east, utm.north, a); + waypoint_move(wp_id, utm.east, utm.north, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c index 6fd1bc7b776..50f1d38d8c7 100644 --- a/sw/airborne/firmwares/rotorcraft/datalink.c +++ b/sw/airborne/firmwares/rotorcraft/datalink.c @@ -108,7 +108,7 @@ void dl_parse_msg(void) */ lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; - nav_move_waypoint_lla(wp_id, &lla); + waypoint_move_lla(wp_id, &lla); } } break; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index b4520fd23bb..0febffb7d81 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/sw/airborne/firmwares/rotorcraft/navigation.c @@ -374,14 +374,14 @@ unit_t nav_reset_reference(void) { ins_reset_local_origin(); /* update local ENU coordinates of global waypoints */ - nav_localize_global_waypoints(); + waypoints_localize_all(); return 0; } unit_t nav_reset_alt(void) { ins_reset_altitude_ref(); - nav_localize_global_waypoints(); + waypoints_localize_all(); return 0; } diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index d0a809a769b..9110f0b501a 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -102,7 +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) ({ nav_set_waypoint_here_2d(_wp); FALSE; }) +#define NavSetWaypointHere(_wp) ({ waypoint_set_here_2d(_wp); FALSE; }) /** Normalize a degree angle between 0 and 359 */ #define NormCourse(x) { \ diff --git a/sw/airborne/subsystems/navigation/waypoints.c b/sw/airborne/subsystems/navigation/waypoints.c index 9eba676e099..16a11d84e73 100644 --- a/sw/airborne/subsystems/navigation/waypoints.c +++ b/sw/airborne/subsystems/navigation/waypoints.c @@ -45,14 +45,14 @@ void waypoints_init(void) /* 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]); + waypoint_set_lla(i, &wp_tmp_lla_i[i]); } else { - nav_set_waypoint_enu_f(i, &wp_tmp_float[i]); + waypoint_set_enu_f(i, &wp_tmp_float[i]); } } } -void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu) +void waypoint_set_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)); @@ -60,11 +60,11 @@ void nav_set_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *enu) 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); + waypoint_globalize(wp_id); } } -void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu) +void waypoint_set_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)); @@ -72,14 +72,14 @@ void nav_set_waypoint_enu_f(uint8_t wp_id, struct EnuCoor_f *enu) 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); + waypoint_globalize(wp_id); } } -void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos) +void waypoint_move_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); + waypoint_set_enu_i(wp_id, new_pos); DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z)); } @@ -89,7 +89,7 @@ void nav_move_waypoint_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos) * 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) +void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y) { if (wp_id < nb_waypoint) { waypoints[wp_id].enu_i.x = x; @@ -97,47 +97,47 @@ void nav_set_waypoint_xy_i(uint8_t wp_id, int32_t x, int32_t 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); + waypoint_globalize(wp_id); } } -void nav_set_waypoint_alt_i(uint8_t wp_id, int32_t alt) +void waypoint_set_alt_i(uint8_t wp_id, int32_t alt) { if (wp_id < nb_waypoint) { waypoints[wp_id].enu_i.z = alt; /* also update ENU float representation */ waypoints[wp_id].enu_f.z = POS_FLOAT_OF_BFP(waypoints[wp_id].enu_i.z); - nav_globalize_local_wp(wp_id); + waypoint_globalize(wp_id); } } -void nav_set_waypoint_alt_f(uint8_t wp_id, float alt) +void waypoint_set_alt_f(uint8_t wp_id, float alt) { if (wp_id < nb_waypoint) { waypoints[wp_id].enu_f.z = alt; /* also update ENU fixed point representation */ waypoints[wp_id].enu_i.z = POS_BFP_OF_REAL(waypoints[wp_id].enu_f.z); - nav_globalize_local_wp(wp_id); + waypoint_globalize(wp_id); } } -void nav_set_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +void waypoint_set_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); + waypoint_localize(wp_id); } -void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *lla) +void waypoint_move_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)) { + waypoint_set_lla(wp_id, lla); + if (waypoint_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, @@ -151,7 +151,7 @@ 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) +void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla) { if (wp_id >= nb_waypoint) { return; @@ -159,36 +159,36 @@ void nav_set_waypoint_latlon(uint8_t wp_id, struct LlaCoor_i *lla) 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); + waypoint_localize(wp_id); } /** set waypoint to current location and altitude */ -void nav_set_waypoint_here(uint8_t wp_id) +void waypoint_set_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()); + if (waypoint_is_global(wp_id)) { + waypoint_set_lla(wp_id, stateGetPositionLla_i()); } else { - nav_set_waypoint_enu_i(wp_id, stateGetPositionEnu_i()); + waypoint_set_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) +void waypoint_set_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()); + if (waypoint_is_global(wp_id)) { + waypoint_set_latlon(wp_id, stateGetPositionLla_i()); } else { - nav_set_waypoint_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y); + waypoint_set_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y); } } -void nav_globalize_local_wp(uint8_t wp_id) +void waypoint_globalize(uint8_t wp_id) { if (state.ned_initialized_i) { struct EcefCoor_i ecef; @@ -199,7 +199,7 @@ void nav_globalize_local_wp(uint8_t wp_id) } /** update local ENU coordinates from its LLA coordinates */ -void nav_localize_global_wp(uint8_t wp_id) +void waypoint_localize(uint8_t wp_id) { if (state.ned_initialized_i) { struct EnuCoor_i enu; @@ -216,12 +216,12 @@ void nav_localize_global_wp(uint8_t wp_id) } /** update local ENU coordinates of global waypoints */ -void nav_localize_global_waypoints(void) +void waypoints_localize_all(void) { uint8_t i = 0; for (i = 0; i < nb_waypoint; i++) { - if (nav_wp_is_global(i)) { - nav_localize_global_wp(i); + if (waypoint_is_global(i)) { + waypoint_localize(i); } } } @@ -233,11 +233,11 @@ void nav_localize_global_waypoints(void) * @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) +struct LlaCoor_i *waypoint_get_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); + waypoint_globalize(wp_id); } return &waypoints[wp_id].lla; } diff --git a/sw/airborne/subsystems/navigation/waypoints.h b/sw/airborne/subsystems/navigation/waypoints.h index 9e64c3d84d0..93fecd22ccd 100644 --- a/sw/airborne/subsystems/navigation/waypoints.h +++ b/sw/airborne/subsystems/navigation/waypoints.h @@ -51,7 +51,7 @@ extern struct Waypoint waypoints[]; #define WaypointAlt(_wp) waypoints[_wp].enu_f.z #define Height(_h) (_h) -static inline bool_t nav_wp_is_global(uint8_t wp_id) +static inline bool_t waypoint_is_global(uint8_t wp_id) { if (wp_id < nb_waypoint) { return bit_is_set(waypoints[wp_id].flags, WP_FLAG_GLOBAL); @@ -75,30 +75,30 @@ static inline void waypoint_clear_global_flag(uint8_t wp_id) 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_set_waypoint_alt_i(uint8_t wp_id, int32_t alt); -extern void nav_set_waypoint_alt_f(uint8_t wp_id, float alt); -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); +extern void waypoint_set_enu_f(uint8_t wp_id, struct EnuCoor_f *enu); +extern void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu); +extern void waypoint_set_xy_i(uint8_t wp_id, int32_t x, int32_t y); +extern void waypoint_set_alt_i(uint8_t wp_id, int32_t alt); +extern void waypoint_set_alt_f(uint8_t wp_id, float alt); +extern void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos); +extern void waypoint_set_lla(uint8_t wp_id, struct LlaCoor_i *lla); +extern void waypoint_move_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); +void waypoint_set_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); +extern void waypoint_set_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); +extern void waypoint_set_here_2d(uint8_t wp_id); /** update global LLA coordinates from its ENU coordinates */ -extern void nav_globalize_local_wp(uint8_t wp_id); +extern void waypoint_globalize(uint8_t wp_id); /** update local ENU coordinates from its LLA coordinates */ -extern void nav_localize_global_wp(uint8_t wp_id); +extern void waypoint_localize(uint8_t wp_id); /** update local ENU coordinates of global waypoints */ -extern void nav_localize_global_waypoints(void); +extern void waypoints_localize_all(void); /** Get LLA coordinates of waypoint. * If the waypoint does not have its global coordinates set, @@ -107,6 +107,6 @@ extern void nav_localize_global_waypoints(void); * @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); +extern struct LlaCoor_i *waypoint_get_lla(uint8_t wp_id); #endif /* WAYPOINTS_H */ diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c index c3b79353096..0b1364758ce 100644 --- a/sw/simulator/nps/nps_ivy_rotorcraft.c +++ b/sw/simulator/nps/nps_ivy_rotorcraft.c @@ -54,7 +54,7 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)), * lla.alt is above ellipsoid in mm */ lla.alt = atoi(argv[5]) - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt; - nav_move_waypoint_lla(wp_id, &lla); + waypoint_move_lla(wp_id, &lla); printf("move wp id=%d x=% .4f, y=% .4f, z=% .4f\n", wp_id, WaypointX(wp_id), WaypointY(wp_id), WaypointAlt(wp_id)); }