Skip to content

Commit

Permalink
[waypoints] remove _f postfix for the most used float functions
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Apr 20, 2015
1 parent 6498d19 commit 3588e6c
Show file tree
Hide file tree
Showing 2 changed files with 104 additions and 44 deletions.
52 changes: 49 additions & 3 deletions sw/airborne/subsystems/navigation/waypoints.c
View file
Expand Up @@ -47,11 +47,57 @@ void waypoints_init(void)
waypoint_set_global_flag(i);
waypoint_set_lla(i, &wp_tmp_lla_i[i]);
} else {
waypoint_set_enu_f(i, &wp_tmp_float[i]);
waypoint_set_enu(i, &wp_tmp_float[i]);
}
}
}

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);
}
return FALSE;
}

void waypoint_set_global_flag(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
SetBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
}
}

void waypoint_clear_global_flag(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
ClearBit(waypoints[wp_id].flags, WP_FLAG_GLOBAL);
}
}

float waypoint_get_x(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
return waypoints[wp_id].enu_f.x;
}
return 0.f;
}

float waypoint_get_y(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
return waypoints[wp_id].enu_f.y;
}
return 0.f;
}

float waypoint_get_alt(uint8_t wp_id)
{
if (wp_id < nb_waypoint) {
return waypoints[wp_id].enu_f.z;
}
return 0.f;
}

void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
{
if (wp_id < nb_waypoint) {
Expand All @@ -64,7 +110,7 @@ void waypoint_set_enu_i(uint8_t wp_id, struct EnuCoor_i *enu)
}
}

void waypoint_set_enu_f(uint8_t wp_id, struct EnuCoor_f *enu)
void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu)
{
if (wp_id < nb_waypoint) {
memcpy(&waypoints[wp_id].enu_f, enu, sizeof(struct EnuCoor_f));
Expand Down Expand Up @@ -111,7 +157,7 @@ void waypoint_set_alt_i(uint8_t wp_id, int32_t alt)
}
}

void waypoint_set_alt_f(uint8_t wp_id, float alt)
void waypoint_set_alt(uint8_t wp_id, float alt)
{
if (wp_id < nb_waypoint) {
waypoints[wp_id].enu_f.z = alt;
Expand Down
96 changes: 55 additions & 41 deletions sw/airborne/subsystems/navigation/waypoints.h
View file
Expand Up @@ -51,62 +51,76 @@ extern struct Waypoint waypoints[];
#define WaypointAlt(_wp) waypoints[_wp].enu_f.z
#define Height(_h) (_h)

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);
}
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 waypoint_set_enu_f(uint8_t wp_id, struct EnuCoor_f *enu);
extern bool_t waypoint_is_global(uint8_t wp_id);
extern void waypoint_set_global_flag(uint8_t wp_id);
extern void waypoint_clear_global_flag(uint8_t wp_id);


/*
* Get waypoint coordinates.
*/
/** Get X/East coordinate of waypoint in meters */
extern float waypoint_get_x(uint8_t wp_id);
/** Get Y/North coordinate of waypoint in meters */
extern float waypoint_get_y(uint8_t wp_id);
/** Get altitude of waypoint in meters (above reference) */
extern float waypoint_get_alt(uint8_t wp_id);

/** 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 *waypoint_get_lla(uint8_t wp_id);


/*
* Set waypoint coordinates.
*/
/** Set local ENU waypoint coordinates */
extern void waypoint_set_enu(uint8_t wp_id, struct EnuCoor_f *enu);
/** Set altitude of waypoint in meters (above reference) */
extern void waypoint_set_alt(uint8_t wp_id, float alt);

/** set waypoint to current location and altitude */
extern void waypoint_set_here(uint8_t wp_id);
/** set waypoint to current horizontal location without modifying altitude */
extern void waypoint_set_here_2d(uint8_t wp_id);

/* functions to set fixedpoint representation directly */
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 waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla);
extern void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla);

/** set waypoint to current location and altitude */
extern void waypoint_set_here(uint8_t wp_id);

/** set waypoint to current horizontal location without modifying altitude */
extern void waypoint_set_here_2d(uint8_t wp_id);
/*
* Move waypoints.
* Basically sets the coordinats and sends the WP_MOVED telemetry message as ack.
* @todo keep this here?
*/

extern void waypoint_move_enu_i(uint8_t wp_id, struct EnuCoor_i *new_pos);
extern void waypoint_move_lla(uint8_t wp_id, struct LlaCoor_i *lla);


/*
* Global(LLA) / Local(ENU) conversions.
*/

/** update global LLA coordinates from its ENU coordinates */
extern void waypoint_globalize(uint8_t wp_id);

/** update local ENU coordinates from its LLA coordinates */
extern void waypoint_localize(uint8_t wp_id);
/** update local ENU coordinates of global waypoints */
/** update local ENU coordinates of all global waypoints */
extern void waypoints_localize_all(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 *waypoint_get_lla(uint8_t wp_id);

#endif /* WAYPOINTS_H */

0 comments on commit 3588e6c

Please sign in to comment.