Skip to content

Commit

Permalink
[waypoints] renamve nav_foo_waypoint_bar to functions to waypoint_foo…
Browse files Browse the repository at this point in the history 
…_bar
  • Loading branch information
@flixr
flixr committed Apr 20, 2015
1 parent 0d3f157 commit ea76fa3
Show file tree
Hide file tree
Showing 7 changed files with 58 additions and 58 deletions.
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/fixedwing/datalink.c
View file
Expand Up @@ -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 */
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/rotorcraft/datalink.c
View file
Expand Up @@ -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;
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/firmwares/rotorcraft/navigation.c
View file
Expand Up @@ -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;
}

Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/rotorcraft/navigation.h
View file
Expand Up @@ -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) { \
Expand Down
72 changes: 36 additions & 36 deletions sw/airborne/subsystems/navigation/waypoints.c
View file
Expand Up @@ -45,41 +45,41 @@ 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));
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);
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));
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);
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));
}
Expand All @@ -89,55 +89,55 @@ 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;
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);
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,
Expand All @@ -151,44 +151,44 @@ 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;
}
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;
Expand All @@ -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;
Expand All @@ -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);
}
}
}
Expand All @@ -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;
}
Expand Down
32 changes: 16 additions & 16 deletions sw/airborne/subsystems/navigation/waypoints.h
View file
Expand Up @@ -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);
Expand All @@ -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,
Expand All @@ -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 */
2 changes: 1 addition & 1 deletion sw/simulator/nps/nps_ivy_rotorcraft.c
View file
Expand Up @@ -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));
}
Expand Down

0 comments on commit ea76fa3

Please sign in to comment.