Skip to content

Commit

Permalink
[navigation] start implementing global waypoints
Browse files Browse the repository at this point in the history 
- each waypoint is a struct with multiple representations and flags (e.g. if absolute/global)
- factor out waypoints in separate files so it can be shared between firmwares later
  • Loading branch information
@flixr
flixr committed Jan 28, 2015
1 parent ce4e12d commit bd297c7
Show file tree
Hide file tree
Showing 7 changed files with 330 additions and 69 deletions.
1 change: 1 addition & 0 deletions conf/firmwares/subsystems/rotorcraft/navigation.makefile
View file
Expand Up @@ -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
2 changes: 1 addition & 1 deletion conf/flight_plans/rotorcraft_basic.xml
View file
Expand Up @@ -12,7 +12,7 @@
<waypoint name="p2" x="27.5" y="-48.2"/>
<waypoint name="p3" x="16.7" y="-19.6"/>
<waypoint name="p4" x="13.7" y="-40.7"/>
<waypoint name="CAM" x="-20" y="-50" height="2."/>
<waypoint name="CAM" lat="43.5636661" lon="1.4810443" height="2."/>
<waypoint name="TD" x="5.6" y="-10.9"/>
</waypoints>
<blocks>
Expand Down
67 changes: 20 additions & 47 deletions sw/airborne/firmwares/rotorcraft/navigation.c
View file
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}

Expand All @@ -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
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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
Expand Down
31 changes: 10 additions & 21 deletions sw/airborne/firmwares/rotorcraft/navigation.h
View file
Expand Up @@ -30,18 +30,15 @@

#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

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);

Expand Down Expand Up @@ -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);

Expand All @@ -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) { \
Expand All @@ -124,15 +113,15 @@ 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); \
}

/*********** Navigation on a circle **************************************/
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)
Expand All @@ -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 */
Expand Down

0 comments on commit bd297c7

Please sign in to comment.