Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

NavCheckWaypoint function added

Browse Source
This commit is contained in:
kadir4172
2014-04-18 13:25:52 +03:00
parent 8d928c51d1
commit 21a6974dd2
2 changed files with 33 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+28 -4
View File
@@ -292,16 +292,13 @@ void nav_route(uint8_t wp_start, uint8_t wp_end) {
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
int32_t dist_to_point;
struct Int32Vect2 diff;
static uint8_t time_at_wp = 0;
VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
INT32_VECT2_NORM(dist_to_point, diff);
//printf("dist %d | %d %d\n", dist_to_point,diff.x,diff.y);
//fflush(stdout);
//if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) time_at_wp++;
else time_at_wp = 0;
if (time_at_wp > 20) return TRUE;
if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)) return TRUE;
if (from_idx > 0 && from_idx < NB_WAYPOINT) {
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, waypoints[wp_idx], waypoints[from_idx]);
@@ -311,6 +308,33 @@ bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx) {
else return FALSE;
}
bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time) {
uint16_t time_at_wp;
int32_t dist_to_point;
static uint16_t wp_entry_time = 0;
static bool_t wp_reached = FALSE;
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[wp_idx], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
INT32_VECT2_NORM(dist_to_point, diff);
if (dist_to_point < (ARRIVED_AT_WAYPOINT >> INT32_POS_FRAC)){
if(!wp_reached) {
wp_reached = TRUE;
wp_entry_time = autopilot_flight_time;
time_at_wp = 0;
}
else {
time_at_wp = autopilot_flight_time - wp_entry_time;
}
}
else {
time_at_wp = 0;
wp_reached = FALSE;
}
if (time_at_wp > stay_time) return TRUE;
return FALSE;
}
static inline void nav_set_altitude( void ) {
static int32_t last_nav_alt = 0;
if (abs(nav_altitude - last_nav_alt) > (POS_BFP_OF_REAL(0.2))) {
sw/airborne/firmwares/rotorcraft/navigation.h
+5
View File
@@ -145,10 +145,15 @@ extern void nav_route(uint8_t wp_start, uint8_t wp_end);
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
}
/** Proximity tests on approaching a wp */
bool_t nav_approaching_from(uint8_t wp_idx, uint8_t from_idx);
#define NavApproaching(wp, time) nav_approaching_from(wp, 0)
#define NavApproachingFrom(wp, from, time) nav_approaching_from(wp, from)
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
bool_t nav_check_wp_time(uint8_t wp_idx, uint16_t stay_time);
#define NavCheckWaypointTime(wp, time) nav_check_wp_time(wp, time)
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
#define NavVerticalAutoThrottleMode(_pitch) { \